perm filename INTERP.PAS[AL,HE]9 blob
sn#714820 filedate 1983-06-06 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00058 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00006 00002 (*$E+ Routines to interpret an AL program *)
C00013 00003 (* datatype definitions *)
C00016 00004 (* statement definitions *)
C00020 00005 (* auxiliary definitions: variable, etc. *)
C00022 00006 (* definition of the ubiquitous NODE record *)
C00029 00007 (* records for parser: ident, token, resword *)
C00032 00008 (* process descriptor blocks & environment record definitions *)
C00036 00009 (* definition of AL-ARM messages *)
C00038 00010 (* global variables *)
C00040 00011 (* external routines *)
C00047 00012 (* message passing routines: sendCmd, sendTrans, getReply, whereArm *)
C00052 00013 (* aux routines: push, pop, upTrans, getELev, getEntry, getVar, gtVarn, getVal, setVal, getNval *)
C00061 00014 (* aux routines: getPdb, freePdb, getEvent, freeEvent *)
C00065 00015 (* graph structure routines: nextTime, getFrame, getDevice, feval, eval, change, invalidate, stvals ... *)
C00077 00016 (* aux routines to create & destroy variables: enterEntry,makeCmon,makeVar,killVar,killEnv,killNode,killStack *)
C00091 00017 (* aux io routines: prntSval, prntVec, prntTrans, prntStrng, prntPlist, onum, prntVar, badjoints *)
C00096 00018 (* aux routines: addPdb, sleep, deClkQueue, ppArmError, msgDispatch, swap *)
C00114 00019 (* aux routines: calibrate,initArms,initWorld,consDef,passConstants,flushLevel,flushAll,unwind,flushPdb,flushKids *)
C00134 00020 (* aux routines: cmonEnable, cmonDisable, cmonCheck *)
C00140 00021 (* expression evaluator: evalExp *)
C00159 00022 procedure doProg (* ** ** *)
C00160 00023 procedure doBlock
C00162 00024 procedure doCoblock
C00165 00025 procedure doEnd
C00170 00026 procedure doFor
C00173 00027 procedure doIf
C00174 00028 procedure doWhile
C00175 00029 procedure doUntil
C00176 00030 procedure doCase
C00178 00031 procedure doCall
C00179 00032 procedure doReturn
C00183 00033 procedure doPrint
C00184 00034 procedure doPrompt
C00186 00035 procedure doPause
C00187 00036 procedure doAbort
C00189 00037 procedure doSay
C00196 00038 procedure doAssign
C00198 00039 procedure doSignal
C00200 00040 procedure doWait
C00202 00041 procedure doEnable
C00203 00042 procedure doDisable
C00204 00043 (* affixment auxiliary routines: affixaux, unfixaux & unfix *)
C00210 00044 procedure doAffix
C00216 00045 procedure doUnfix
C00217 00046 (* aux routines for motions: forcebits, getMechbits, moveStart, moveEnd, moveRetry *)
C00232 00047 procedure doCmon
C00239 00048 procedure doMove
C00268 00049 procedure doOperate
C00272 00050 procedure doOpen (* & doClose *)
C00279 00051 procedure doCenter
C00281 00052 procedure doArmmagic
C00285 00053 procedure doFloat
C00288 00054 procedure doStop
C00290 00055 procedure doRetry
C00292 00056 procedure doSetbase
C00294 00057 procedure doWrist
C00298 00058 (* command loop *)
C00310 ENDMK
C⊗;
(*$E+ Routines to interpret an AL program *)
(*$S4000 use an even larger codesize (was using 3000) *)
program interp;
const
version = 10; (* 10 for simulation version, 11 for real thing *)
(* the following other routines need to be manually *)
(* changed when running with the servo: *)
(* procedure bitOn (in Calibrate) *)
(* procedure initWorld (rewrite at end) *)
(* Also the external definitions for the various *)
(* procedures from RSXMSG need to be fixed *)
(* The following bits are used during calls to the ARM servo *)
GARMDEV = 1; (* device numbers for ARM *)
GHANDDEV = 2;
RARMDEV = 3;
RHANDDEV = 4;
DRIVERDEV = 5;
VISEDEV = 6;
FTABLE = (*400B*) 256; (* Force trans (C) in table coordinates *)
FHAND = 0; (* " " " " hand coordinate system *)
XFORCE = 0; (* Force along X direction of C *)
YFORCE = (*1000B*) 512; (* " " Y " " " *)
ZFORCE = (*2000B*) 1024; (* " " Z " " " *)
XMOMENT = (*3000B*)1536; (* Moment about X direction of C *)
YMOMENT = (*4000B*)2048; (* " " Y " " " *)
ZMOMENT = (*5000B*)2560; (* " " Z " " " *)
FSTOP = (*10000B*)4096; (* In addition to starting cmon, stop arm *)
SIGMAG = (*20000B*)8192; (* Test only magnitude of forces *)
SIGGE = (*100000B*) 32768; (* Start cmon if force ≥ specified value *)
SIGLT = 0; (* " " " " < " " *)
garmpower = 1; (* bit defs - used in response to initarmscmd *)
garmcal = 2;
ghandpower = 4;
ghandcal = (* 10B *) 8;
rarmpower = (* 20B *) 16;
rarmcal = (* 40B *) 32;
rhandpower = (* 100B *) 64;
rhandcal = (* 200B *) 128;
(* control bits for trajectory specs: movesegcmd & movehdrcmd *)
Viaptcb = 1; Joint1cb = 1; (* 1B *)
Deptptcb = 2; Joint2cb = 2; (* 2B *)
Apprptcb = 4; Joint3cb = 4; (* 4B *)
Destptcb = 8; Joint4cb = 8; (* 10B *)
Veloccb = 16; Joint5cb = 16; (* 20B *)
Codecb = 32; Joint6cb = 32; (* 40B *)
Durlbcb = 64; (* 100B *)
Durubcb = 128; (* 200B *)
Dureqcb = 192; (* 300B *)
Byptcb = 256; Linearcb = 256; (* 400B *)
Nullingcb = 512; (* 1000B *)
Shouldercb = 1024; Wobblecb = 1024; (* 2000B *)
Rightcb = 2048; Speedfcb = 2048; (* 4000B *)
Elbowcb = 4096; Loadcb = 4096; (* 10000B *)
Upcb = 8192; (* 20000B *)
Wristcb = 16384; (* 40000B *)
Flipcb = 32768; (* 100000B *)
maxInt = 32767; (* max 16 bit integer *)
(* Control character definitions and others *)
ctlA = 01; (* Control-A *)
ctlB = 02;
ctlC = 03;
ctlD = 04;
ctlE = 05;
ctlF = 06;
ctlG = 07;
ctlH = 08;
ctlI = 09;
ctlJ = 10;
ctlK = 11;
ctlL = 12;
ctlM = 13;
ctlN = 14;
ctlO = 15;
ctlP = 16;
ctlQ = 17;
ctlR = 18;
ctlS = 19;
ctlT = 20;
ctlU = 21;
ctlV = 22;
ctlW = 23;
ctlX = 24;
ctlY = 25;
ctlZ = 26;
FF = ctlL; (* Form feed *)
CR = ctlM; (* Carriage return *)
LF = ctlJ; (* Line feed *)
TAB = ctlI; (* Tab *)
ESC = 27; (* Escape *)
smallA = 97; (* Lowercase a (sail pascal converts all input to upper case) *)
smallZ = 122;
undline = 95; (* Underline _ *)
vbar = 124; (* Vertical bar | *)
lbrace = 123; (* Left brace (curly bracket) *)
rbrace = 126; (* and right brace *)
deletekey = 127; (* Delete key code *)
type
(* random type declarations for OMSI/SAIL compatibility *)
(* ascii = char; *)
atext = packed file of ascii;
(* atext = text; *)
(* Here are all the pointer-type definitions. Since the various *)
(* records reference each other so much, we have to put them all here. *)
vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
statementp = ↑statement;
varidefp = ↑varidef;
nodep = ↑node;
identp = ↑ident;
tokenp = ↑token;
reswordp = ↑resword;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
cmoncbp = ↑cmoncb;
messagep = ↑message;
(* datatype definitions *)
datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
frametype, eventtype, strngtype, labeltype, proctype, arraytype,
reftype, valtype, cmontype, nulltype, undeftype,
dimensiontype, mactype, macargtype, freevartype);
scalar = real;
vector = record refcnt: integer; val: array [1..3] of real end;
trans = record refcnt: integer; val: array [1..3,1..4] of real end;
cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
strng = record
next: strngp;
ch: cstring;
end;
event = record
next: eventp; (* all events are on one big list *)
count: integer;
waitlist: pdbp;
end;
frame = record
vari: varidefp; (* back pointer to variable name & info *)
calcs: nodep; (* affixment info *)
case ftype: boolean of (* frame = true, device = false *)
true: (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
false: (mech: integer; case sdev: boolean of
true: (sdest: real); false: (tdest,appr,depr: transp));
(* sdev = true for scalar devices, false for frames *)
end;
byte = 0..255; (* doesn't really belong here, but... *)
(* statement definitions *)
stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
fortype, iftype, whiletype, untiltype, casetype,
calltype, returntype,
printtype, prompttype, pausetype, aborttype, assigntype,
signaltype, waittype, enabletype, disabletype, cmtype,
affixtype, unfixtype,
movetype,jtmovetype,operatetype,opentype,closetype,centertype,
floattype, stoptype, retrytype,
requiretype, definetype, macrotype, commenttype, dimdeftype,
setbasetype, wristtype, saytype, declaretype, emptytype,
evaltype, armmagictype);
(* more??? *)
statement = packed record
next, last: statementp;
stlab: varidefp;
exprs: nodep; (* any expressions used by this statement *)
nlines: integer;
bpt,bad: boolean;
case stype: stmntypes of
progtype: (pcode: statementp; errors: integer);
blocktype,
declaretype,
endtype,
coendtype: (bcode, bparent: statementp; blkid: identp;
level, numvars: 0..255; variables: varidefp);
coblocktype: (threads: nodep; nthreads: integer; cblkid: identp);
fortype: (forvar, initial, step, final: nodep; fbody: statementp);
whiletype,
untiltype: (cond: nodep; body: statementp);
casetype: (index: nodep; range, ncases: integer; caselist: nodep);
iftype: (icond: nodep; thn, els: statementp);
pausetype: (ptime: nodep);
prompttype,
printtype,
aborttype,
saytype: (plist: nodep; debugLev: integer);
returntype: (retval, rproc: nodep);
evaltype,
calltype,
assigntype: (what, aval: nodep);
affixtype,
unfixtype: (frame1, frame2, byvar, atexp: nodep; rigid: boolean);
signaltype,
waittype: (event: nodep);
movetype,
jtmovetype,
operatetype,
opentype,
closetype,
centertype,
floattype,
setbasetype,
stoptype: (cf, clauses: nodep);
retrytype: (rcode, rparent: statementp; olevel: integer);
wristtype: (arm, ff, fvec, tvec: nodep; csys: boolean);
cmtype: (oncond: nodep; conclusion: statementp;
deferCm, exprCm: boolean; cdef: varidefp);
enabletype,
disabletype: (cmonlab: varidefp);
requiretype: (rfil: boolean; rfils: strngp; rfilen: integer);
definetype: (macname,mpars: varidefp; macdef: tokenp);
commenttype: (len: integer; str: strngp; cbody: statementp);
dimdeftype: (dimname: varidefp; dimexpr: nodep);
armmagictype: (cmdnum,dev,iargs,oargs: nodep);
end;
(* auxiliary definitions: variable, etc. *)
varidef = packed record
next,dnext: varidefp;
name: identp;
level: 0..255; (* environment level *)
offset: 0..255; (* environment offset *)
dtype: varidefp; (* to hold the dimension info *)
tbits: 0..15; (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
dbits: 0..15; (* for use by debugger/interpreter *)
case vtype: datatypes of
arraytype: (a: nodep);
proctype: (p: nodep);
labeltype,
cmontype: (s: statementp);
mactype: (mdef: statementp);
macargtype: (marg: tokenp);
pconstype: (c: nodep);
dimensiontype: (dim: nodep);
end;
(* definition of the ubiquitous NODE record *)
nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
deprnode, viaptnode, apprnode, destnode, byptnode, durnode,
sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
arrivalnode, departingnode,
ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
calcnode, arraydefnode, bnddefnode, bndvalnode,
waitlistnode, procdefnode, tlistnode, dimnode, commentnode,
linearnode, elbownode, shouldernode, flipnode, wrtnode,
loadnode,velocitynode);
exprtypes = ( svalop, (* scalar operators *)
sltop, sleop, seqop, sgeop, sgtop, sneop, (* relations *)
notop, orop, xorop, andop, eqvop, (* logical *)
saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
sexpop, maxop, minop, intop, idivop, modop,
sqrtop, logop, expop, timeop, (* functions *)
sinop, cosop, tanop, asinop, acosop, atan2op, (* trig *)
vdotop, vmagnop, tmagnop,
vecop, (* vector operators *)
vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
svmulop, vsmulop, vsdivop, tvmulop, wrtop,
tposop, taxisop,
transop, (* trans operators *)
tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
ioop, (* i/o operators *)
queryop, inscalarop,
specop, (* special operators *)
arefop, callop, grinchop, macroop, vmop, adcop, dacop, jointop,
badop,
addop, subop, negop, mulop, divop, absop); (* for parsing *)
leaftypes = pconstype..strngtype;
reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);
node = record
next: nodep;
case ntype: nodetypes of
exprnode: (op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
leafnode: (case ltype: leaftypes of
varitype: (vari: varidefp; vid: identp);
pconstype: (cname: varidefp; pcval: nodep);
svaltype: (s: scalar; wid: integer);
vectype: (v: vectorp);
transtype: (t: transp);
strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
listnode: (lval: nodep);
clistnode: (cval: integer; stmnt: statementp; clast: nodep);
colistnode: (prev: nodep; cstmnt: statementp);
forvalnode: (fvar: enventryp; fstep: scalar; fstmnt: statementp);
arrivalnode:(evar: varidefp);
wrtnode,
deprnode,
apprnode,
destnode: (loc: nodep; code: statementp);
byptnode,
viaptnode: (vlist: boolean; via,vclauses: nodep; vcode: statementp);
durnode: (durrel: reltypes; durval: nodep);
velocitynode,
sfacnode,
wobblenode,
swtnode: (clval: nodep);
nullingnode, (* true = nonulling *)
wristnode, (* = don't zero force wrist *)
cwnode, (* = counter_clockwise *)
elbownode, (* = elbow up *)
shouldernode, (* = right shoulder *)
flipnode, (* = don't flip wrist *)
linearnode: (notp: boolean); (* = linear motion *)
ffnode: (ff,cf: nodep; csys, pdef: boolean); (* true = world, false = hand *)
loadnode: (loadval,loadvec: nodep; lcsys: boolean); (* lcsys = csys above *)
forcenode: (ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
stiffnode: (fv, mv, cocff: nodep);
gathernode: (gbits: integer);
cmonnode: (cmon: statementp; errhandlerp: boolean);
errornode: (eexpr: nodep);
calcnode: (rigid, frame1: boolean; other: framep; case tvarp: boolean of
false: (tval: transp); true: (tvar: enventryp) );
arraydefnode: (numdims: 1..10; bounds: nodep; combnds: boolean);
bnddefnode: (lower, upper: nodep);
bndvalnode: (lb, ub, mult: integer);
waitlistnode: (who: pdbp; when: integer);
procdefnode:(ptype: datatypes; level: 0..255;
pname, paramlist: varidefp; body: statementp);
tlistnode: (tok: tokenp);
dimnode: (time, distance, angle, dforce: integer);
end;
(* records for parser: ident, token, resword *)
ident = record
next: identp;
length: integer;
name: strngp;
curv: varidefp;
end;
tokentypes = (reswdtype, identtype, constype, comnttype, delimtype, labeldeftype,
macpartype);
constypes = svaltype..strngtype;
reswdtypes = (stmnttype, filtype, clsetype, decltype, optype);
filtypes = (abouttype,alongtype,attype,bytype,defertype,dotype,elsetype,
errmodestype,fromtype,handtype,intype,nonrigidlytype,rigidlytype,
sourcefiletype,steptype,thentype,totype,untltype,viatype,
withtype,worldtype,zeroedtype,oftype,wheretype,nowaittype);
clsetypes = (approachtype,arrivaltype,departuretype,departingtype,durationtype,
errortype,forcetype,forceframetype,forcewristtype,gathertype,
nonullingtype,nullingtype,stiffnesstype,torquetype,velocitytype,
wobbletype,cwtype,ccwtype,stopwaittimetype,angularvelocitytype);
token = record
next: tokenp;
case ttype: tokentypes of
constype: (cons: nodep);
comnttype: (len: integer; str: strngp);
delimtype: (ch: char);
reswdtype: (case rtype: reswdtypes of
stmnttype: (stmnt: stmntypes);
filtype: (filler: filtypes);
clsetype: (clause: clsetypes);
decltype: (decl: datatypes);
optype: (op: exprtypes) );
identtype: (id: identp);
labeldeftype: (lab: varidefp);
macpartype: (mpar: varidefp);
end;
resword = record
next: reswordp;
length: integer;
name: strngp;
case rtype: reswdtypes of
stmnttype: (stmnt: stmntypes);
filtype: (filler: filtypes);
clsetype: (clause: clsetypes);
decltype: (decl: datatypes);
optype: (op: exprtypes);
end;
(* process descriptor blocks & environment record definitions *)
queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
forcewait,devicewait,joinwait,proccall);
pdb = packed record
nextpdb,next: pdbp; (* for list of all/active pdb's *)
level: 0..255; (* lexical level *)
mode: 0..255; (* expression/statement/sub-statement *)
priority: 0..255;
status: queuetypes; (* what are we doing *)
env: envheaderp;
spc: statementp; (* current statement *)
epc: nodep; (* current expression (if any) *)
sp: nodep; (* intermediate value stack *)
cm: cmoncbp; (* if we're a cmon point to our definition *)
mech: framep; (* current device being used *)
linenum: integer; (* used by editor/debugger *)
case procp: boolean of (* true if we're a procedure *)
true: (opdb: pdbp; (* pdb to restore when procedure exits *)
pdef: nodep); (* procedure definition node *)
false: (evt: eventp; (* event to signal when process goes away *)
sdef: statementp); (* first statement where process was defined *)
end;
envheader = packed record
parent: envheaderp;
env: array [0..4] of environp;
varcnt: 0..255; (* # of variables in use ??? *)
case procp: boolean of (* true if we're a procedure *)
true: (proc: nodep);
false:(block: statementp);
end;
enventry = record
case etype: datatypes of
svaltype: (s: scalar);
vectype: (v: vectorp);
transtype: (t: transp);
frametype: (f: framep);
eventtype: (evt: eventp);
strngtype: (length: integer; str: strngp);
cmontype: (c: cmoncbp);
proctype: (p: nodep; penv: envheaderp);
reftype: (r: enventryp);
arraytype: (a: envheaderp; bnds: nodep);
end;
environment = record
next: environp;
vals: array [0..9] of enventryp;
end;
cmoncb = record
running, enabled: boolean; (* cmon's status *)
cmon: statementp;
pdb: pdbp;
evt: eventp;
fbits: integer; (* bits for force sensing *)
oldcmon: cmoncbp; (* for debugger *)
end;
(* definition of AL-ARM messages *)
msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
abortcmd,stopcmd,movehdrcmd,movesegcmd,
centercmd,operatecmd,movedonecmd,signalcmd,
setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
errorcmd,floatcmd,setloadcmd,
armmagiccmd,realcmd,vectorcmd,transcmd);
errortypes = (noerror,noarmsol,timerr,durerr,toolong,featna,
unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
baddev,timout,panicb,nocart,cbound,badparm);
message = record
cmd: msgtypes;
ok: boolean;
case integer of
1: (dev, bits, n: integer;
(* (dev, bits, n, evt: integer; (* for arm code version *)
evt: eventp;
dur: real;
case integer of
1: (v1,v2,v3: real);
2: (sfac,wobble,pos: real);
3: (val,angle,mag: real);
4: (max,min: real);
5: (error: errortypes));
2: (fv1,fv2,fv3,mv1,mv2,mv3: real); (* may never use these... *)
3: (t: array [1..6] of real);
end;
interr = record
case integer of
0: (i: integer);
1: (err,foo: errortypes);
end;
(* global variables *)
var curInt, activeInts, readQueue, allPdbs: pdbp;
sysEnv: envheaderp;
clkQueue: nodep;
allEvents: eventp;
resched, running, escapeI, iSingleThreadMode: boolean;
etime: integer; (* used by eval *)
curtime: integer; (* who knows where this will get updated - an ast? *)
stime: integer; (* used for clock queue on 10 *)
msg: messagep; (* for AL-ARM interaction *)
msgp: boolean; (* flag set if any messages pending *)
inputLine: array [1..20] of ascii;
inputp: integer; (* current offset into inputLine array above *)
inputReady: boolean;
debugLevel: integer;
talk: text; (* for using the speech synthesizer *)
(* various constant pointers *)
xhat,yhat,zhat,nilvect: vectorp;
niltrans: transp;
gpark, rpark: transp; (* arm park positions *)
(* various device & variable pointers *)
speedfactor: enventryp;
garm: framep;
(* external routines *)
procedure initAlloc; extern; (* from ALLOC.PAS *)
function newVector: vectorp; extern;
procedure relVector(v: vectorp); extern;
function newTrans: transp; extern;
procedure relTrans(t: transp); extern;
function newNode: nodep; extern;
procedure relNode(n: nodep); extern;
function newEvent: eventp; extern;
procedure relEvent(n: eventp); extern;
function newEentry: enventryp; extern;
procedure relEentry(n: enventryp); extern;
function newCmoncb: cmoncbp; extern;
procedure relCmoncb(n: cmoncbp); extern;
function newstrng: strngp; extern;
procedure relstrng(n: strngp); extern;
function newIdent: identp; extern;
procedure relIdent(n: identp); extern;
function newVaridef: varidefp; extern;
procedure relVaridef(n: varidefp); extern;
function newFrame: framep; extern;
procedure relFrame(n: framep); extern;
function newEheader: envheaderp; extern;
procedure relEheader(n: envheaderp); extern;
function newStatement: statementp; extern;
procedure relStatement(n: statementp); extern;
function newPdb: pdbp; extern;
procedure relPdb(n: pdbp); extern;
function newEnvironment: environp; extern;
procedure relEnvironment(n: environp); extern;
function sind(d: real): real; extern; (* from ARITH.PAS *)
function cosd(d: real): real; extern;
function tand(d: real): real; extern;
function asin(x: real): real; extern;
function acos(x: real): real; extern;
function atan2(x,y: real): real; extern;
function vdot (u,v: vectorp): scalar; extern;
function vmagn (v: vectorp): scalar; extern;
function vmake (a,b,c: scalar): vectorp; extern;
function svmul (s: scalar; v: vectorp): vectorp; extern;
function vsdiv (v: vectorp; s: scalar): vectorp; extern;
function vadd (u,v: vectorp): vectorp; extern;
function vsub (u,v: vectorp): vectorp; extern;
function unitv (v: vectorp): vectorp; extern;
function vcross (u,v: vectorp): vectorp; extern;
function tvmul (t: transp; v: vectorp): vectorp; extern;
function tpos (t: transp): vectorp; extern;
function torient (t: transp): transp; extern;
function taxis (t: transp): vectorp; extern;
function tmagn (t: transp): scalar; extern;
function tmake (t: transp; v: vectorp): transp; extern;
function tvadd (t: transp; v: vectorp): transp; extern;
function tvsub (t: transp; v: vectorp): transp; extern;
function ttmul (t1,t2: transp): transp; extern;
function tinvrt (t: transp): transp; extern;
function vsaxwr(ax: vectorp; w: real): transp; extern;
function construct(org,vx,vxy: vectorp): transp; extern;
function vmkfrc(v: vectorp): transp; extern;
function getsysVars: varidefp; extern; (* from PARSE.PAS *)
(* function startArm: boolean; extern; (* from RSXMSG.PAS *)
(* procedure initMsg(var buf: messagep; var flag: boolean); extern;
function SendArm: boolean; extern;
function GetArm: boolean; extern;
procedure signalArm; extern; *)
function startArm: boolean; begin startArm := true; end;
procedure initMsg(var buf: messagep; var flag: boolean);
begin new(buf); buf↑.ok := true end; (* for simulation version *)
function sendArm: boolean; begin sendArm := true; end;
function getArm: boolean; begin getArm := true; end;
procedure signalArm; begin end;
procedure ppLine; extern; (* from EDIT.PAS *)
procedure ppOutNow; extern;
procedure ppChar(ch: ascii); extern;
procedure pp5(ch: c5str; length: integer); extern;
procedure pp10(ch: cstring; length: integer); extern;
procedure pp10L(ch: cstring; length: integer);extern;
procedure pp20(ch: c20str; length: integer); extern;
procedure pp20L(ch: c20str; length: integer); extern;
procedure ppInt(i: integer); extern;
procedure ppReal(r: real); extern;
procedure ppStrng(length: integer; s: strngp); extern;
procedure ppDelChar; extern;
function anyChar(var ch: ascii): boolean; extern; (* from DISP.FAI *)
function getChar: ascii; extern;
procedure escInit(var flg: boolean); extern;
procedure beep; extern;
function getCurInt: pdbp; (* SAIL - for use by EDIT *)
begin
getCurInt := curInt;
end;
procedure setCurInt(p: pdbp);
begin
curInt := p;
end;
function getAllPdbs: pdbp;
begin
getAllPdbs := allPdbs;
end;
procedure setSingleThreadMode(b: boolean);
begin
iSingleThreadMode := b;
end;
(* message passing routines: sendCmd, sendTrans, getReply, whereArm *)
procedure sendCmd;
var b: boolean;
begin
b := sendArm; (* send message to ARM *)
with msg↑ do
if not (cmd in [movesegcmd, movehdrcmd, setccmd, wristcmd, setstiffcmd,
armmagiccmd, realcmd, vectorcmd, transcmd]) then
signalArm; (* tell ARM *)
end;
procedure sendTrans(tr: transp);
var i,j,k: integer; b: boolean;
begin
b := sendArm; (* first send over message header *)
with msg↑,tr↑ do
begin
for k := 0 to 1 do
begin
for i := 1 to 3 do
for j := 1 to 2 do t[i + 3*(j-1)] := val[i,j + 2*k];
b := sendArm; (* send half over *)
end;
if refcnt <= 0 then relTrans(tr);
end;
end;
procedure msgDispatch; forward; (* handles signals & movedone from ARM *)
procedure getReply(sendIt: boolean);
var ocmd: msgtypes; b: boolean;
begin
with msg↑ do
begin
ocmd := cmd; (* remember what we're waiting for *)
if sendIt then sendCmd; (* send request to ARM servo *)
repeat
b := getArm; (* try to read a message packet from ARM *)
if b and (cmd <> ocmd) then (* if we got one, was it our reply? *)
begin
msgDispatch; (* deal with whatever the ARM servo sent over *)
b := false; (* keep waiting for our reply *)
end
until b; (* wait for reply *)
end;
end;
function getEntry (level, offset: byte): enventryp; forward;
procedure ppArmError(err: errortypes; angle: integer); forward;
function whereArm (mech: integer): transp; (* to read in the arm's position *)
var tp: transp; i,j: integer; b: boolean;
ev: enventryp; (* for sim ver *)
begin
tp := newTrans;
with msg↑,tp↑ do
begin
cmd := wherecmd;
dev := mech;
bits := 0;
getReply(true); (* go get 1st message packet *)
if ok then (* check there's no error *)
begin
for i := 1 to 3 do
for j := 1 to 2 do val[i,j] := t[i + 3*(j-1)]; (* copy result *)
repeat b := getArm until b; (* get 2nd packet (guaranteed to be next) *)
for i := 1 to 3 do
for j := 3 to 4 do val[i,j] := t[i + 3*(j-3)]; (* copy result *)
if version = 10 then
begin (* for simulation version *)
relTrans(tp);
case mech of (* get device offset *)
GARMDEV: i := 0;
RARMDEV: i := 4;
end;
ev := getEntry(0,i);
tp := ev↑.f↑.tdest; (* use wherever last move was to *)
end
end
else
begin (* ERROR - complain *)
ppArmError(error,bits);
relTrans(tp); (* don't need this anymore *)
tp := niltrans;
end;
end;
whereArm := tp;
end;
(* aux routines: push, pop, upTrans, getELev, getEntry, getVar, gtVarn, getVal, setVal, getNval *)
procedure push (n: nodep);
begin (* no need to check for overflow *)
n↑.next := curInt↑.sp;
curInt↑.sp := n;
end;
function pop: nodep;
begin
pop := curInt↑.sp;
if curInt↑.sp = nil then
begin (* **** error - stack underflow **** *)
pp20L('Value Stack Underflo',20); ppChar('w'); ppLine;
(* code to show where error occurred & to maybe recover??? *)
end
else curInt↑.sp := curInt↑.sp↑.next;
end;
procedure upTrans (var t: transp; tp: transp);
begin
if tp <> nil then tp↑.refcnt := tp↑.refcnt + 1; (* indicate new trans is in use *)
if t <> nil then (* check for old value *)
begin
t↑.refcnt := t↑.refcnt - 1; (* we're done with trans now *)
if t↑.refcnt <= 0 then relTrans(t); (* release it if no one else wants it *)
end;
t := tp; (* copy new trans pointer *)
end;
function envlookup (offset: integer; envhdr: envheaderp): enventryp;
var i,j,k: integer; env: environp;
begin
i := offset div 10; (* which environment block *)
j := offset mod 10; (* entry in environment block *)
if i < 5 then env := envhdr↑.env[i] (* use direct look-up *)
else begin (* run through linked list *)
env := envhdr↑.env[4];
for k := 5 to i do env := env↑.next;
end;
envlookup := env↑.vals[j];
end;
function getELev(hdr: envheaderp): integer;
begin
if hdr = sysEnv then getELev := 0
else if hdr↑.procp then getELev := hdr↑.proc↑.level
else getELev := hdr↑.block↑.level;
end;
function getEntry (* (level, offset: byte): enventryp; *);
var hdr: envheaderp;
begin
if level = 0 then hdr := sysEnv (* level zero is predefined system variables *)
else
begin
hdr := curInt↑.env; (* look up the env entry given level-offset *)
while level < getELev(hdr) do hdr := hdr↑.parent; (* move up a level *)
if level <> getELev(hdr) then (* yow!!! no environment exists!!! *)
begin
pp20L('Attempt to access no',20); pp20('n-existent environme',20);
pp20('nt - good luck! ',16); ppLine;
end;
end;
getEntry := envlookup(offset,hdr);
end;
function getVar (level, offset: byte): enventryp;
var entry: enventryp; i, j: integer; p, b: nodep;
begin
entry := getEntry(level,offset); (* get the environment entry *)
while entry↑.etype = reftype do entry := entry↑.r; (* resolve indirect refs *)
if entry↑.etype = arraytype then (* do array reference *)
begin
b := entry↑.bnds;
j := 0;
repeat
p := pop; (* get this subscript's value *)
i := round(p↑.s);
relNode(p);
if i < b↑.lb then (* subscript error *)
begin
pp20L('Subscript index less',20); pp20(' than lower bound: ',19);
ppInt(i); ppLine;
i := b↑.lb
end
else if i > b↑.ub then (* subscript error *)
begin
pp20L('Subscript index grea',20); pp20('ter than lower bound',20);
pp5(': ',2); ppInt(i); ppLine;
i := b↑.ub
end;
j := j + b↑.mult * (i - b↑.lb);
b := b↑.next;
until b = nil;
entry := envlookup(j,entry↑.a); (* lookup the array entry *)
end;
getVar := entry;
end;
function gtVarn (n: nodep): enventryp;
begin
with n↑ do
if ntype = leafnode then
with vari↑ do gtVarn := getVar(level,offset) (* access simple var *)
else
with arg1↑.vari↑ do gtVarn := getVar(level,offset); (* access array var *)
end;
procedure getFrame (f: framep; r: nodep); forward;
procedure getVal (level, offset: byte);
var entry: enventryp; res: nodep;
begin
entry := getVar(level,offset); (* look up environment entry for variable *)
res := newNode;
res↑.ntype := leafnode;
res↑.ltype := entry↑.etype; (* copy datatype of result *)
if entry↑.etype = svaltype then res↑.s := entry↑.s (* it's a scalar *)
else if entry↑.etype <> frametype then (* it's a vector, trans or string *)
with res↑ do
begin
v := entry↑.v; (* copy pointer *)
str := entry↑.str;
if v = nil then
if ltype = vectype then v := nilvect
else if ltype = transtype then t := niltrans
else length := 0;
(* complain??? *)
end
else
begin
res↑.ltype := transtype;
getFrame(entry↑.f,res);
end;
push(res); (* store the value on the stack *)
end;
procedure change (f: framep; res: nodep); forward;
procedure setVal (level, offset: byte);
var entry: enventryp; res: nodep;
begin
entry := getVar(level,offset); (* look up environment entry for variable *)
res := pop; (* pop value off of stack *)
with entry↑ do
if etype = svaltype then s := res↑.s (* it's a scalar *)
else if etype = vectype then
begin
with res↑.v↑ do refcnt := refcnt + 1; (* indicate new vector is in use *)
if v <> nil then
begin
v↑.refcnt := v↑.refcnt - 1; (* we're done with vector now *)
if v↑.refcnt <= 0 then relVector(v); (* release it if no one wants it *)
end;
v := res↑.v; (* copy new vector pointer *)
end
else if etype = transtype then upTrans(t,res↑.t) (* update trans with new value *)
else if etype = strngtype then
begin
length := res↑.length;
str := res↑.str; (* copy new string pointer *)
end
else change(f,res); (* change frame's value, updating affixed frames *)
relNode(res); (* free node up *)
end;
function getNval(n: nodep; var b: boolean): nodep;
begin
b := false;
with n↑ do
if (ntype <> leafnode) or (ltype = varitype) then
begin n := pop; b := true end;
if n <> nil then
if n↑.ltype = pconstype then
begin n := n↑.pcval; b := false end;
getNval := n;
end;
(* aux routines: getPdb, freePdb, getEvent, freeEvent *)
function getPdb: pdbp;
var p: pdbp;
begin
p := newPdb;
with p↑ do
begin (* initialize it somewhat *)
nextPdb := allPdbs;
allPdbs := p; (* add us to list of all processes *)
next := nil;
if curInt <> nil then
begin
env := curInt↑.env;
level := getELev(env) + 1;
priority := curInt↑.priority;
cm := curInt↑.cm;
end
else
begin
env := sysEnv;
level := 1;
priority := 0;
cm := nil;
end;
status := nullqueue;
mode := 0;
spc := nil;
epc := nil;
sp := nil;
mech := nil;
procp := false;
evt := nil;
end;
getPdb := p;
end;
procedure freePdb(p: pdbp);
var po: pdbp; b: boolean;
begin (* remove pdb from list *)
if allPdbs <> nil then
if allPdbs = p then allPdbs := p↑.nextPdb
else
begin
po := allPdbs;
b := false;
repeat (* find pdb in list *)
if po↑.nextPdb = p then b := true else po := po↑.nextPdb
until b or (po = nil);
if b then po↑.nextPdb := p↑.nextPdb; (* splice us out of list *)
(* *** else complain??? *** *)
end;
relPdb(p);
end;
function getEvent: eventp;
var e: eventp;
begin
e := newEvent;
e↑.next := allEvents; (* add to list of all events *)
allEvents := e;
e↑.count := 0;
e↑.waitlist := nil;
getEvent := e;
end;
procedure freeEvent(e: eventp);
var eo: eventp; b: boolean;
begin (* remove event from list *)
if allEvents <> nil then
begin
if allEvents = e then begin allEvents := e↑.next; b := true end
else if allEvents <> nil then
begin
eo := allEvents;
b := false;
repeat (* find event in list *)
if eo↑.next = e then b := true else eo := eo↑.next
until b or (eo = nil);
if b then eo↑.next := e↑.next; (* splice us out of list *)
end;
if b then relEvent(e); (* if not in list already released *)
end;
end;
(* graph structure routines: nextTime, getFrame, getDevice, feval, eval, change, invalidate, stvals ... *)
procedure nextTime;
begin
if etime = Maxint then etime := 1 (* should reset all invalid frames, but ... *)
else etime := etime + 1;
end;
procedure eval (f: framep);
var calc: nodep; b: boolean; f2, tr: transp;
begin
if f↑.valid <> etime then (* Haven't looked at it yet *)
begin
f↑.valid := etime; (* Mark it *)
calc := f↑.calcs; (* Get list of calculators *)
b := true;
while (calc <> nil) and b do (* See if someone it's affixed to is now valid *)
if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
with calc↑.other↑ do (* A possibility, look at other frame *)
begin
if not ftype then (* See if it's a device or frame *)
begin (* It's a device - use it to compute current value *)
f2 := whereArm(mech); (* Get current device pos *)
b := false; (* No need to look further *)
end
else if (dcntr=0) and (valid=0) then (* not dynamic & valid frame *)
begin f2 := val; b := false end
else calc := calc↑.next (* dynamic or not valid - try next *)
end
else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)
if calc = nil then
begin (* Check calcs again - this time trying to evaluate other frame *)
calc := f↑.calcs;
b := true;
while (calc <> nil) and b do
if (calc↑.ntype = calcnode) and (calc↑.rigid or calc↑.frame1) then
begin
eval(calc↑.other); (* Try to get a value for it *)
if calc↑.other↑.valid=0 then (* Is it now valid? *)
begin f2 := calc↑.other↑.val; b := false end (* Yes - all done *)
else calc := calc↑.next (* still not valid - try next *)
end
else calc := calc↑.next; (* not a calc, or nonrigid and frame2 - try next *)
end;
if calc <> nil then
with calc↑ do
begin (* use other frame to evaluate desired one & return success *)
if tvarp then tr := tvar↑.t else tr := tval; (* explicitly named trans? *)
if not frame1 then tr := tinvrt(tr); (* second := inv(trans) * first *)
upTrans(f↑.val,ttmul(tr,f2)); (* first := trans * second *)
f↑.valid := 0; (* Mark it as now valid *)
end;
end;
end;
function feval (f: framep): transp;
begin
if not f↑.ftype then
begin (* If device use its current value *)
feval := whereArm(f↑.mech); (* Get current device pos *)
end
else (* frame *)
begin
if (f↑.dcntr<>0) or (f↑.valid<>0) then (* dynamic frame or not valid? *)
begin (* Need to calculate current value *)
nextTime; (* update eval time *)
eval(f); (* try to evaluate the variable *)
end;
if f↑.valid = 0 then feval := f↑.val (* copy trans pointer *)
else feval := niltrans; (* but always return something *)
end;
end;
function invalidate (f: framep): boolean;
var calc: nodep; b: boolean;
begin
(* invalidate frame & all other frames affixed either rigidly or
non-rigidly with this being frame2,
else indicate we need to modify non-rigid trans. *)
b := false; (* assume no updating of non-rigid relationships *)
if etime <> f↑.valid then (* haven't marked this one yet *)
with f↑ do
begin
if valid = 0 then upTrans(val,nil); (* flush old value *)
valid := etime; (* mark us as having an invalid value *)
calc := calcs;
while calc <> nil do (* invalidate everyone we're affixed to *)
begin (* rigidly or if we're frame 2 *)
if (calc↑.ntype = calcnode) and (calc↑.rigid or (not calc↑.frame1))
then b := b or invalidate(calc↑.other) (* go invalidate frame *)
else b := true; (* found a non-rigid affixment to update *)
calc := calc↑.next; (* now repeat with next calc *)
end;
end;
invalidate := b;
end;
procedure stvals (f: framep);
var calc,c2: nodep; t,val: transp; f2: framep;
begin
calc := f↑.calcs;
val := f↑.val; (* frames current value *)
while calc <> nil do (* update everyone we're affixed to *)
with calc↑ do
begin
f2 := other;
if (ntype = calcnode) and (rigid or (not frame1)) then
begin (* see if we need to update this frame *)
if f2↑.valid <> 0 then (* haven't updated it yet *)
begin
if tvarp then t := tvar↑.t else t := tval; (* explicitly named trans? *)
if frame1 then t := tinvrt(t); (* second := inv(trans) * first *)
upTrans(f2↑.val,ttmul(t,val)); (* first := trans * second *)
f2↑.valid := 0; (* Mark it as now valid *)
stvals(f2); (* and go update its affixments *)
end
end
else
begin (* need to update relation trans *)
t := feval(f2); (* get a value for f2 *)
t := ttmul(val,tinvrt(t)); (* compute new relation trans *)
if tvarp then upTrans(tvar↑.t,t)
else
begin
upTrans(tval,t); (* store it *)
c2 := f2↑.calcs; (* now go fix trans up in f2's calc list *)
while c2↑.other <> f do c2 := c2↑.next; (* find other calc of pair *)
upTrans(c2↑.tval,t); (* copy trans to it too *)
end;
end;
calc := calc↑.next; (* move on to next one *)
end;
end;
procedure change (* f: framep; res: nodep *);
var calc: nodep; b: boolean;
begin
if f↑.dcntr=0 then (* if not dynamic *)
begin
nextTime;
b := invalidate(f); (* b = true if any non-rigid affixments need updating *)
f↑.val := res↑.t; (* copy trans pointer *)
f↑.val↑.refcnt:=f↑.val↑.refcnt + 1; (* mark trans in use *)
f↑.valid := 0; (* mark us as having a valid value *)
if b then stvals(f); (* go fix up the non-rigid relationships *)
end
else begin
pp20L('Can''t assign to dyna',20); pp10('mic frames',10); ppLine;
(* maybe also give name of frame?? *)
end;
end;
procedure getDevice (f: framep; r: nodep);
var i: integer; ev: enventryp; (* for sim ver *)
begin
if f↑.sdev then
with msg↑ do
begin
cmd := wherecmd;
dev := f↑.mech;
bits := 0;
getReply(true); (* have ARM servo read in the hand/device value *)
if ok then r↑.s := t[1]
else
begin (* ERROR - complain *)
ppArmError(error,bits);
r↑.s := 0;
end;
r↑.ltype := svaltype;
if version = 10 then
begin (* for simulation version *)
case dev of (* get device offset *)
GHANDDEV: i := 2;
RHANDDEV: i := 6;
DRIVERDEV: i := 8;
VISEDEV: i := 12;
end;
ev := getEntry(0,i);
r↑.s := ev↑.f↑.sdest; (* use where ever last move was to *)
end;
end
else
r↑.t := whereArm(f↑.mech); (* go read in the arm's position *)
end;
procedure getFrame (* f: framep; r: nodep *);
begin
if not f↑.ftype then getDevice(f,r) (* If device get its current value *)
else (* frame *)
begin
if (f↑.dcntr<>0) or (f↑.valid<>0) then (* dynamic frame or not valid? *)
begin (* Need to calculate current value *)
nextTime; (* update eval time *)
eval(f); (* try to evaluate the variable *)
end;
r↑.t := f↑.val; (* copy trans pointer *)
if r↑.t = nil then r↑.t := niltrans; (* always return something *)
(* complain though??? *)
end;
end;
(* aux routines to create & destroy variables: enterEntry,makeCmon,makeVar,killVar,killEnv,killNode,killStack *)
function enterEntry (var i,j: integer; var env: environp;
envhdr: envheaderp; v: varidefp): enventryp;
var e: enventryp; k: integer;
begin
if j = 9 then (* need to allocate new environment record *)
begin
env↑.next := newEnvironment;
env := env↑.next;
env↑.next := nil;
for k := 0 to 9 do env↑.vals[k] := nil;
j := 0;
i := i + 1;
if i < 5 then envhdr↑.env[i] := env;
end
else j := j + 1;
k := 10 * i + j;
if k > envhdr↑.varcnt then envhdr↑.varcnt := k;
e := newEentry; (* get an environment entry for the variable *)
env↑.vals[j] := e;
e↑.etype := v↑.vtype; (* copy datatype of variable *)
if e↑.etype = rottype then e↑.etype := transtype; (* rots are transes internally *)
enterEntry := e;
end;
procedure makeCmon(e: enventryp; vari: varidefp);
var c: cmoncbp;
begin
c := newCmoncb;
with c↑ do
begin
cmon := vari↑.s; (* point to cmon definition *)
enabled := false;
running := false;
pdb := getPdb; (* get us a pdb for later *)
oldcmon := e↑.c; (* remember if we're pushing anyone *)
if c↑.cmon↑.oncond↑.ntype = forcenode then
evt := getEvent (* we'll need an event later *)
else evt := nil;
end;
with c↑.pdb↑ do
begin (* set up pdb *)
priority := (priority mod 10) + 1; (* base level priority *)
spc := c↑.cmon;
sdef := spc;
cm := c; (* point to cmon def *)
opdb := curInt; (* pointer to parent pdb so we can get mech bits *)
end;
e↑.c := c;
end;
procedure makeVar(e: enventryp; vari: varidefp; tbits: integer);
var i,j,k,size: integer; envhdr: envheaderp; env: environp; ep: enventryp;
b,bo,bd: nodep;
function getBound (n: nodep): integer;
var e: enventryp;
begin
if n↑.ntype = exprnode then (* value on stack *)
begin n := pop; getBound := round(n↑.s) end
else if n↑.ltype = svaltype then getBound := round(n↑.s) (* constant val *)
else if n↑.ltype = pconstype then
getBound := round(n↑.pcval↑.s) (* predeclared constant *)
else
begin (* variable value *)
with n↑.vari↑ do e := getVar(level,offset);
getBound := round(e↑.s);
end;
end;
function getSize (b: nodep): integer;
begin
if b↑.next = nil then b↑.mult := 1
else b↑.mult := getSize(b↑.next);
getSize := b↑.mult * (b↑.ub - b↑.lb + 1);
end;
begin
with e↑ do
begin
if tbits = 1 then etype := arraytype
else if tbits = 2 then etype := proctype
else if tbits >= 4 then etype := reftype;
case etype of
svaltype: s := 0.0;
vectype,
transtype: v := nil;
frametype: begin
f := newFrame;
f↑.vari := vari;
f↑.calcs := nil;
f↑.ftype := true;
f↑.valid := -1;
f↑.val := nil;
f↑.fdepr := nil;
f↑.dcntr := 0;
f↑.dev := nil;
end;
eventtype: evt := getEvent;
strngtype: begin length := 0; str := nil end;
cmontype: begin
c := nil;
makeCmon(e,vari);
end;
proctype: begin
etype := proctype; (* fix up type field *)
p := vari↑.p;
penv := curInt↑.env;
end;
arraytype: begin
bd := vari↑.a↑.bounds;
bo := nil;
while bd <> nil do (* bind the array bounds *)
begin
b := newNode;
if bo = nil then e↑.bnds := b else bo↑.next := b;
bo := b;
with b↑ do
begin
next := nil;
ntype := bndvalnode;
lb := getBound(bd↑.lower);
ub := getBound(bd↑.upper);
end;
bd := bd↑.next
end;
size := getSize(e↑.bnds);
envhdr := newEheader;
envhdr↑.varcnt := 0;
e↑.a := envhdr;
env := newEnvironment;
env↑.next := nil;
envhdr↑.env[0] := env;
for j := 1 to 4 do envhdr↑.env[j] := nil;
for j := 0 to 9 do env↑.vals[j] := nil;
i := 0;
j := -1;
for k := 1 to size do
begin
ep := enterEntry(i,j,env,envhdr,vari);
makeVar(ep,vari,0); (* make variable environment entry *)
end;
for i := j+1 to 9 do env↑.vals[i] := nil;
end;
end;
end;
end;
procedure unfix(f1,f2: framep); forward;
procedure flushPdb(p: pdbp); forward;
procedure addPdb(var plist: pdbp; pn: pdbp); forward;
procedure killVar(e: enventryp);
var j,k,size: integer; envhdr: envheaderp; env,eo: environp; ep: enventryp;
b,bo: nodep; pp: pdbp; cp: cmoncbp;
begin
with e↑ do
case etype of
svaltype,
strngtype: begin end; (* nothing to do *)
vectype: if v <> nil then (* check for old value *)
begin
v↑.refcnt := v↑.refcnt - 1; (* we're done with vector now *)
if v↑.refcnt <= 0 then relVector(v); (* release it if no one else wants it *)
end;
transtype: upTrans(t,nil);
frametype: begin
while f↑.calcs <> nil do
unfix(f,f↑.calcs↑.other); (* unfix us from everyone *)
upTrans(f↑.val,nil); (* flush our current value *)
relFrame(f); (* flush frame *)
end;
eventtype: begin
(* *** what to do with those processes waiting on this event? *** *)
pp := evt↑.waitlist;
while pp <> nil do
begin pp↑.status := nullqueue; pp := pp↑.next end;
freeEvent(evt);
end;
cmontype: repeat
if c↑.cmon↑.oncond↑.ntype = forcenode then freeEvent(c↑.evt);
flushPdb(c↑.pdb); (* now it's ok to flush its pdb *)
cp := c↑.oldcmon; (* did we have several copies active? *)
relCmoncb(c); (* and also free up its cmoncb *)
c := cp;
until cp = nil;
arraytype: begin
b := e↑.bnds;
size := b↑.mult * (b↑.ub - b↑.lb + 1); (* get array size *)
while b <> nil do begin bo := b; b := b↑.next; relNode(bo) end;
envhdr := e↑.a;
env := envhdr↑.env[0];
relEheader(envhdr);
j := -1;
for k := 1 to size do
begin
if j = 9 then
begin eo := env; env := env↑.next; relEnvironment(eo); j := 0 end
else j := j + 1;
ep := env↑.vals[j];
killVar(ep); (* kill variable environment entry *)
end;
relEnvironment(env);
end;
proctype: begin (* return to any processes waiting for procedure *)
pp := allPdbs;
while pp <> nil do (* run through all the active processes *)
with pp↑ do
begin
if procp and (pdef = p) then
begin
b := newNode;
with b↑ do (* return default value *)
begin
ntype := leafnode;
ltype := p↑.ptype; (* copy datatype of result *)
if ltype = svaltype then s := 0.0 (* it's a scalar *)
else if ltype = vectype then v := nilvect
else if ltype = transtype then t := niltrans
else begin length := 0; str := nil end;
next := opdb↑.sp; (* put it on stack *)
opdb↑.sp := b;
end;
opdb↑.status := runqueue;
addPdb(activeInts,opdb); (* re-activate caller *)
end;
pp := nextPdb;
end;
end;
others: (* nothing to do for indirect references *)
end;
relEentry(e);
e := nil;
end;
procedure killEnv;
var envhdr: envheaderp; envir,eo: environp; e: enventryp; j: integer;
begin
if (curInt↑.env <> sysEnv) and (curInt↑.env↑.varcnt < 255) then
begin (* varcnt check is so flushall doesn't have us kill it twice *)
with curInt↑ do
begin
envhdr := env;
env := envhdr↑.parent;
end;
envhdr↑.varcnt := 255;
envir := envhdr↑.env[0];
relEheader(envhdr);
j := 0;
while envir <> nil do (* deallocate variables *)
begin
e := envir↑.vals[j];
if e <> nil then killVar(e); (* kill var's environment entry *)
if j = 9 then
begin
eo := envir;
envir := envir↑.next;
relEnvironment(eo);
j := 0
end
else j := j + 1;
end;
end
else curInt↑.env := sysEnv;
end;
procedure killNode(n: nodep);
begin
with n↑ do
if ntype = leafnode then
case ltype of
vectype: if v↑.refcnt <= 0 then relVector(v);
transtype: if t↑.refcnt <= 0 then relTrans(t);
others: begin end; (* nothing to do *)
end;
relNode(n);
end;
procedure killStack;
var n,np: nodep;
begin
n := curInt↑.sp; (* top of stack *)
while n <> nil do
begin
np := n↑.next;
killNode(n);
n := np;
end;
curInt↑.sp := nil;
end;
(* aux io routines: prntSval, prntVec, prntTrans, prntStrng, prntPlist, onum, prntVar, badjoints *)
procedure prntSval(s: real);
var si: real;
begin
if s < maxInt then
begin
si := trunc(s);
s := si + round(1000*(s-si))/1000;
end;
ppReal(s);
end;
procedure prntVec(v: vectorp);
var i: integer;
begin
pp10('vector( ',7);
with v↑ do
for i := 1 to 3 do
begin
prntSval(val[i]);
if i = 3 then ppChar(')') else ppChar(',');
end;
ppOutNow;
end;
procedure prntTrans(t: transp);
var i: integer; v: vectorp;
begin
with t↑ do
begin
refcnt := refcnt + 1;
pp10('trans(rot(',10);
v := taxis(t); prntVec(v); relVector(v);
ppChar(',');
prntSval(tmagn(t));
pp10('),vector( ',9);
for i := 1 to 3 do
begin prntSval(val[i,4]); if i = 3 then ppChar(')') else ppChar(',') end;
ppChar(')');
refcnt := refcnt - 1;
end;
ppLine;
end;
procedure prntStrng(length: integer; s: strngp);
begin
ppStrng(length,s);
ppOutNow;
end;
procedure prntPlist(n: nodep);
var np: nodep; b: boolean;
begin
while n <> nil do (* print out the list *)
begin
np := getNval(n↑.lval,b);
if np <> nil then
begin
with np↑ do
case ltype of
svaltype: begin prntSval(s); ppOutNow end;
vectype: prntVec(v);
transtype: prntTrans(t);
strngtype: prntStrng(length,str);
end;
if b then killNode(np); (* flush used stack entry *)
end;
n := n↑.next;
end;
end;
procedure onum(s: integer);
procedure onum1(s: integer);
var i,j: integer;
begin
i := s div 8;
j := s mod 8;
if i > 0 then onum(i);
ppInt(j);
end;
begin
if s < 0 then begin ppChar('-'); s := -s end;
onum1(s);
ppOutNow;
end;
procedure prntVar(v: nodep);
var i: integer; n,p: nodep;
begin
if v = nil then pp10('Noname ',6)
else if v↑.ntype = leafnode then
with v↑.vid↑ do ppStrng(length,name) (* print variable name *)
else
begin (* array ref *)
with v↑.arg1↑.vid↑ do ppStrng(length,name); (* print variable name *)
n := v↑.arg2;
ppChar('[');
while n <> nil do
begin
p := pop; (* get this subscript's value *)
i := round(p↑.s);
ppInt(i);
relNode(p);
n := n↑.next;
if n = nil then ppChar(']') else ppChar(',');
end;
end;
ppLine;
end;
procedure badJoints(angle: integer);
var i: integer;
begin
if angle <> 0 then
begin (* tell associated joint numbers *)
pp20(' joint(s) = ',14);
i := 1;
while angle <> 0 do (* decode them *)
begin
if odd(angle) then
begin
ppInt(i);
if angle > 1 then ppChar(',');
end;
angle := angle div 2;
i := i + 1;
end;
ppLine;
end;
end;
(* aux routines: addPdb, sleep, deClkQueue, ppArmError, msgDispatch, swap *)
procedure addPdb (* var plist: pdbp; pn: pdbp *);
var p,pp: pdbp; b: boolean;
begin
if plist = nil then
begin (* empty queue - we're it *)
plist := pn;
pn↑.next := nil;
end
else if plist↑.priority < pn↑.priority then
begin (* add us to start of queue *)
pn↑.next := plist;
plist := pn;
end
else
begin (* merge us into the queue *)
p := plist;
b := true;
while (p↑.next <> nil) and b do
if p↑.next↑.priority >= pn↑.priority then p := p↑.next else b := false;
pn↑.next := p↑.next;
p↑.next := pn;
end;
end;
procedure sleep(whenV: integer);
var w,n,np: nodep; p,pp: pdbp; b: boolean; ti: integer;
begin
curInt↑.next := nil;
np := clkQueue;
n := nil;
b := true;
ti := stime;
while np <> nil do
if ti = whenV then (* add us to this wait node *)
begin
addPdb(np↑.who,curInt);
np := nil;
b := false;
end
else if ti < whenV then
begin (* move down list *)
whenV := whenV - ti; (* update relative wait time *)
n := np;
np := np↑.next;
if np <> nil then ti := np↑.when;
end
else np := nil;
if b then (* need to make a new entry *)
begin
w := newNode;
with w↑ do
begin
ntype := waitlistnode;
who := curInt;
when := whenV;
next := nil;
end;
(* request a Marktime ast to have us made active *)
if n = nil then
begin
w↑.next := clkQueue;
clkQueue := w; (* first entry in queue *)
stime := whenv; (* hack for 10 *)
end
else
begin (* add us to the queue *)
w↑.next := n↑.next;
n↑.next := w;
end;
if w↑.next <> nil then w↑.next↑.when := w↑.next↑.when - whenV;
end;
curInt↑.status := sleepqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
procedure deClkQueue(po: pdbp);
var n,np: nodep; p,pp: pdbp; b: boolean;
begin (* remove pdb from clock queue *)
if po↑.status = sleepqueue then po↑.status := nullqueue;
n := clkQueue;
np := nil;
b := true;
while (n <> nil) and b do
begin
p := n↑.who;
pp := nil;
while (p <> nil) and (p <> po) do begin pp := p; p := p↑.next end;
if p <> nil then (* found us, now splice us out of the list *)
begin
b := false;
if pp = nil then
begin (* we were first entry in list *)
n↑.who := p↑.next;
if n↑.who = nil then (* check if we were only entry *)
begin (* yup - remove this wait list node *)
if np <> nil then np↑.next := n↑.next (* splice out node *)
else
begin (* we were first node *)
clkQueue := n↑.next;
if n↑.next = nil then stime := 0 (* clock queue empty now *)
else stime := stime + n↑.next↑.when; (* reset new wait time *)
end;
if n↑.next <> nil then n↑.next↑.when := n↑.when + n↑.next↑.when;
relNode(n); (* done with waitlist node now *)
end
end
else pp↑.next := p↑.next; (* splice us out of list *)
end
else begin np := n; n := n↑.next end; (* try next node *)
end;
end;
procedure ppArmError (* err: errortypes; angle: integer *);
begin
if err = nopower then
begin pp20('arm power not on ',16); ppLine; end
else if err = devbusy then
begin pp20('device currently in ',20); pp5('use ',4); ppLine end
else
begin
case err of
srvdead: pp10('servo dead',10);
adcdead: pp10('a/d error ',9);
panicb: pp20('panic button pushed ',19);
exjtfc: begin pp20('excessive joint forc',20); ppChar('e'); end;
timout: pp10('time out ',8);
paslim: pp20('joint out of range ',18);
badpot: pp20('bad pot on PUMA ',15);
noarmsol: pp20('No arm solution ',16);
nocart: begin pp20('No Cartesian path ex',20); pp20('ists between these p',20);
pp20('ath points. ',11) end;
timerr: begin pp20('Specified motion tim',20); pp20('e exceeds capabiliti',20);
pp5('es. ',3) end;
durerr: begin pp20('Motion overly constr',20); pp20('ained, will ignore g',20);
pp20('lobal time constrain',20); pp5('t. ',2) end;
toolong: begin pp20('Maximum segment time',20); pp20(' allowed is 32.2 sec',20);
pp5('onds.',5) end;
badparm: pp20('Bad Magic Parameter ',19);
unkmess: begin pp20('Unknown Message Type',20); pp20(' received from AL! ',18) end;
nozind: begin pp20('No Zero Index found ',20); pp20('( PUMA Encoder ) ',16) end;
baddev: begin pp20('Device can''t perform',20); pp20(' commanded action ',17) end;
cbound: begin pp20('ARM Code compute bou',20); pp5('nd! ',3) end;
featna: begin pp20('Feature not availabl',20); pp10('e yet. ',6) end;
others: begin pp20('Unknown error! = ',17); ppInt(ord(err)) end;
end;
badJoints(angle); (* tell which joint(s) were bad, if any *)
end;
end;
procedure msgDispatch; (* handles signals & movedone from ARM *)
var p,po: pdbp; nd,np: nodep; nvari: varidefp; i,j,k: integer; b: boolean;
entry: enventryp;
begin
with msg↑ do
if cmd = errorcmd then
begin
if ok then pp20L('Fatal error: ',13)
else pp10L('Warning: ',9);
case dev of (* tell which device *)
garmdev: pp10('garm - ',7);
ghanddev: pp10('ghand - ',8);
rarmdev: pp10('rarm - ',7);
rhanddev: pp10('rhand - ',8);
driverdev: pp10('driver - ',9);
visedev: pp10('vise - ',7);
others: pp20('unknown device - ',18);
end;
ppArmError(error,bits);
end
else (* *** really should check that msg type is legit, but.... *** *)
begin
evt↑.count := evt↑.count + 1;
p := evt↑.waitlist; (* get pdb of process to schedule (if any) *)
if p <> nil then
begin
evt↑.waitlist := p↑.next; (* remove node from waitlist *)
p↑.status := runqueue;
addPdb(activeInts,p); (* add it to active process list *)
if curInt = nil then resched := true
else
if p↑.priority > curInt↑.priority then
resched := true; (* swap it in and swap us out *)
if cmd = movedonecmd then
begin (* need to put error bits on stack *)
nd := newNode;
with nd↑ do
begin
ntype := leafnode;
ltype := svaltype;
if ok then s := 0 else s := 128 * ord(error) + bits;
next := p↑.sp; (* push it *)
p↑.sp := nd;
end;
freeEvent(evt); (* also need to reclaim event *)
end
else if cmd = armmagiccmd then
begin
po := curInt;
curInt := p; (* swap process in temporarily *)
j := n; (* get number of args being passed back *)
np := p↑.spc↑.oargs;
for i := 1 to j do
begin (* get the results of the arm magic cmd *)
repeat until getArm; (* read next message *)
b := np <> nil;
if b then
with np↑.lval↑ do
if ntype = leafnode then nvari := vari else nvari := arg1↑.vari;
nd := newNode;
nd↑.ntype := leafnode;
nd↑.ltype := svaltype; (* (so killnode will be happy) *)
if cmd = realcmd then
begin
nd↑.s := dur; (* copy returned scalar *)
if b then b := nvari↑.vtype = svaltype;
end
else if cmd = vectorcmd then
begin
nd↑.ltype := vectype;
nd↑.v := newVector; (* copy returned vector *)
with nd↑.v↑ do
begin val[1] := v1; val[2] := v2; val[3] := v3 end;
if b then b := nvari↑.vtype = vectype;
end
else if cmd = transcmd then
begin
nd↑.ltype := transtype;
nd↑.t := newTrans; (* copy returned trans *)
with nd↑.t↑ do
begin
for k := 1 to 3 do begin t[k] := val[k,1]; t[k+3] := val[k,2] end;
repeat until getArm; (* read second packet of trans *)
(* ??? should probably check that it's a transcmd, but.... ??? *)
for k := 1 to 3 do begin t[k] := val[k,3]; t[k+3] := val[k,4] end;
end;
if b then b := nvari↑.vtype in [rottype,transtype,frametype];
end
else
begin (* !!! this should never happen!!! *)
pp20L('Bad message received',20); pp20(' during ARM MAGIC - ',20);
pp10('Good Luck!',10); ppLine;
b := false;
end;
if np = nil then killNode(nd) (* flush unwanted value *)
else
begin
if b then
begin (* store value away in variable *)
push(nd); (* first push value onto stack *)
with nvari↑ do
setVal(level,offset); (* store value into variable *)
end
else
begin
pp20L('Datatype of value re',20); pp20('turned from ARM MAGI',20);
pp20('C does not match ',16); ppLine;
killNode(nd); (* flush unwanted value *)
with nvari↑ do (* pop any subscripts off of stack *)
entry := getVar(level,offset); (* look up env entry *)
(* ??? should we zero it instead of leaving it unchanged ??? *)
end;
np := np↑.next;
end;
end;
if np <> nil then
begin
pp20L('Not enough values pa',20); pp20('ssed back from ARM M',20);
pp5('AGIC ',4); ppLine;
while np <> nil do
begin (* clear any subscripts off of the stack *)
with np↑.lval↑ do
if ntype <> leafnode then
with arg1↑.vari↑ do
entry := getVar(level,offset); (* look up env entry *)
(* ??? should we zero it instead of leaving it unchanged ??? *)
np := np↑.next;
end
end;
curInt := po; (* restore current process *)
end
else if cmd <> signalcmd then
begin pp20('Unknown message of t',20); pp5('ype: ',5);
ppInt(ord(cmd)); ppLine end;
end;
end;
end;
procedure swap(newp: pdbp);
var p,po: pdbp; b: boolean; e: eventp;
begin
if newp = nil then
begin (* swap in some active process *)
curInt := activeInts;
if activeInts <> nil then activeInts := activeInts↑.next;
end
else
begin
if newp↑.status = runqueue then
begin (* remove us from activeInts list *)
if activeInts = newp then activeInts := newp↑.next;
p := activeInts;
while p↑.next <> nil do
if p↑.next = newp then p↑.next := newp↑.next (* remove us *)
else p := p↑.next;
end
else if newp↑.status = sleepqueue then deClkQueue(newp)
else if newp↑.status = eventqueue then
begin (* run through all events & remove us from event queue *)
e := allEvents;
b := true;
while b and (e <> nil) do
with e↑ do
begin
if waitlist = newp then
begin waitlist := newp↑.next; b := false end
else
begin
p := waitlist;
while b and (p <> nil) do
if p↑.next = newp then
begin p↑.next := newp↑.next; b := false end
else p := p↑.next;
end;
if b then e := next else count := count + 1;
end;
end;
if (newp <> curInt) and (curInt <> nil) then
begin
curInt↑.status := runqueue;
addPdb(activeInts,curInt); (* swap current process out *)
end;
curInt := newp; (* make new guy active *)
newp↑.next := nil;
end;
if curInt <> nil then
begin curInt↑.status := nowrunning; curInt↑.next := nil end;
end;
(* aux routines: calibrate,initArms,initWorld,consDef,passConstants,flushLevel,flushAll,unwind,flushPdb,flushKids *)
function getPromptChar: ascii;
var ch: ascii;
begin
repeat ch := getChar until ord(ch) <> lf; (* Read one character *)
if ord(ch) = cr then ch := ' '; (* Convert CR to space *)
ppChar(ch); ppOutNow; (* and echo it *)
if (smallA <= ord(ch)) and (ord(ch) <= smallZ) then
ch := chr(ord(ch)-ord(' ')); (* To upper case *)
getPromptChar := ch;
end;
procedure calibrate;
var b: boolean; i,calbits: integer; ch: ascii;
function bitOn(i: integer): boolean;
begin bitOn := true end; (* *** simulation version *** *)
(* begin bitOn := (msg↑.bits AND i) <> 0 end; (* *** non-standard Pascal *** *)
procedure whichArm;
begin
case i of (* tell which arm/hand *)
1: pp5('GARM ',4);
2: pp5('GHAND',5);
3: pp5('RARM ',4);
4: pp5('RHAND',5);
end;
end;
function powerOn: boolean;
var b: boolean;
begin
case i of
1: b := bitOn(garmpower);
2: b := bitOn(ghandpower);
3: b := bitOn(rarmpower);
4: b := bitOn(rhandpower);
end;
powerOn := b;
end;
begin (* hand-shaking code to calibrate arms *)
begin
for i := 1 to 4 do (* try to init just the PUMA's & hands for now *)
begin
repeat
with msg↑ do
begin
ch := ' ';
cmd := initarmscmd;
case i of
1: begin dev := garmdev; calbits := garmcal end;
2: begin dev := ghanddev; calbits := ghandcal end;
3: begin dev := rarmdev; calbits := rarmcal end;
4: begin dev := rhanddev; calbits := rhandcal end;
end;
getReply(true); (* send over init command & wait for reply *)
b := ok and powerOn;
if not ok then
pp20L('Couldn''t initialize ',20)
else if not b then
pp20L('Power off for ',14);
if not b then begin whichArm; ppOutNow end;
if ok then (* try to calibrate PUMA's *)
begin
while not b do (* get power turned on *)
begin
pp20L('Turn on arm high pow',20); pp20 ('er (Type SPACE to co',20);
pp20 ('ntinue, any other to',20); pp10 (' abort): ',9);
ppOutNow;
ch := getPromptChar;
if ch <> ' ' then (* any letter will abort *)
begin
pp10L(' Aborted ',8);
if not bitOn(calbits) then pp20(' - not calibrated ',17);
ppLine;
ppOutNow;
b := true; (* so we leave power up loop *)
end
else
begin (* keep trying *)
getReply(true); (* retry the init command & check power *)
b := ok and powerOn;
end;
end;
if ch <> ' ' then b := false
else b := bitOn(calbits);
if (ch = ' ') and not b then (* if not already calibrated ... *)
begin
pp20L('Type Y to calibrate ',20); whichArm;
ppOutNow;
ch := getPromptChar;
if (ch = 'Y') then
begin
cmd := calibcmd;
getReply(true); (* go calibrate arm *)
b := ok;
if b then pp20L('Calibration complete',20)
else begin pp20L('Error while calibrat',20); pp5('ing ',3); end;
end
else begin pp20L(' Aborted - not calib',20); pp5('rated',5); end;
ppLine; ppOutNow;
end;
end;
end;
if not b then
begin
pp20L('Type Y to try again:',20); ppchar(' '); ppOutNow;
ch := getPromptChar;
b := (ch <> 'Y');
end
until b;
end;
end;
end;
procedure initArms;
var b: boolean;
begin
initMsg(msg,msgp); (* connect to message buffer *)
(* b := startArm; (* get ARM servo running *)
(* *** *) b := true; (* Someday this will work... *)
if b then
begin
if version = 11 then
begin
pp20L('Type "Y" to calibrat',20); pp10('e arms: ',8);
ppOutNow;
if getPromptChar = 'Y' then calibrate;
ppLine;
end
end
else
begin (* Complain if error during startup *)
(* Probably should set some global flag so we don't try to talk to ARM *)
(* or maybe even exit the program *)
pp20L('Error during ARM sta',20); pp20('rtup! Arms not init',20);
pp10('ialized. ',8); ppLine end;
end;
procedure consDef;
begin
xhat := vmake(1,0,0); xhat↑.refcnt := 1000;
yhat := vmake(0,1,0); yhat↑.refcnt := 1000;
zhat := vmake(0,0,1); zhat↑.refcnt := 1000;
nilvect := vmake(0,0,0); nilvect↑.refcnt := 1000;
niltrans := tmake(vsaxwr(zhat,0.0),nilvect); niltrans↑.refcnt := 1000;
(* ypark := tmake(vsaxwr(yhat,180.0),vmake(43.5,2.325,6.86)); *)
(* bpark := tmake(vsaxwr(yhat,180.0),vmake(43.53125,56.855,9.95875)); *)
gpark := tmake(vsaxwr(zhat,180.0),vmake(83.2,46.13,67.7));
rpark := tmake(niltrans,vmake(84.8,12.87,67.7));
gpark↑.refcnt := 1000;
rpark↑.refcnt := 1000;
end;
procedure passConstants(var x,y,z,nv: vectorp; var g,r,nt: transp);
begin
x := xhat; y := yhat; z := zhat; nv := nilvect;
g := gpark; r := rpark; nt := niltrans;
end;
procedure initWorld;
var v: varidefp; e: enventryp; i,j: integer; envir: environp;
b: boolean;
begin
initArms; (* *** should this go here ??? *** *)
etime := 0;
curtime := 0;
activeInts := nil; (* zero the various queues *)
clkQueue := nil;
readQueue := nil;
allPdbs := nil;
curInt := nil;
allEvents := nil;
resched := false;
iSingleThreadMode := false;
sysEnv := newEheader; (* set up system variables *)
with sysEnv↑ do
begin
parent := nil;
block := nil;
procp := false;
envir := newEnvironment;
env[0] := envir;
for i := 1 to 4 do env[i] := nil;
end;
i := 0;
j := -1;
v := getsysVars; (* get list of predefined system variables *)
while v <> nil do
begin
(* need to handle devices specially - especially scalar devices *)
e := enterEntry(i,j,envir,sysEnv,v);
b := v↑.offset in [0,2,4,6,8,12];
(* offsets: arms: 0,4 hands: 2,6 driver/vise: 8,12 *)
if b then e↑.etype := frametype; (* so we get a frame for scalar devices *)
makeVar(e,v,v↑.tbits); (* make variable environment entry *)
if b then (* set up device values *)
with e↑.f↑ do
begin
ftype := false; (* it's a device *)
sdev := v↑.vtype = svaltype; (* indicate if scalar *)
if sdev then sdest := 0
else
begin
tdest := niltrans;
appr := nil;
depr := nil;
end;
case v↑.offset div 2 of (* set Mechanism bits *)
0: mech := GARMDEV; (* garm *)
1: mech := GHANDDEV; (* ghand *)
2: mech := RARMDEV; (* rarm *)
3: mech := RHANDDEV; (* rhand *)
4: mech := DRIVERDEV; (* driver *)
6: mech := VISEDEV; (* vise *)
end;
end;
v := v↑.next
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
speedfactor := getEntry(0,16);
e := getEntry(0,0); (* offset for garm = 0 *)
garm := e↑.f; (* remember frame used for green arm *)
curInt := getPdb;
escInit(escapeI); (* enable escape-I interrupts *)
if version = 11 then
begin
(* rewrite(talk,'tt3:'); (* *** *) (* speech synthesizer is on tt3: *)
writeln(talk,chr(5),'20P'); (* Set up standard AL voice (a la jjc) *)
end;
end;
procedure flushLevel(dLev: integer); (* to clean up from debugger *)
var b: boolean; pri: integer; e: eventp; pp,po: pdbp; ee: enventryp;
begin
pri := dLev * 10;
if curInt <> nil then
if curInt↑.priority >= pri then curInt := nil;
b := true;
while b and (activeInts <> nil) do (* flush run queue *)
if activeInts↑.priority >= pri then activeInts := activeInts↑.next
else b := false;
b := true;
while b and (readQueue <> nil) do (* flush read queue *)
if readQueue↑.priority >= pri then readQueue := readQueue↑.next
else b := false;
e := allEvents;
while e <> nil do
with e↑ do
begin
b := true;
while b and (waitlist <> nil) do (* clean up event's waitlist *)
if waitlist↑.priority >= pri then
begin
waitlist := waitlist↑.next;
count := count + 1;
end
else b := false;
e := next;
end;
po := curInt;
pp := allPdbs;
while pp <> nil do
begin
curInt := pp;
pp := pp↑.nextPdb;
with curInt↑ do
if priority >= pri then (* may need to flush this one *)
begin
killStack;
while level < getELev(env) do killEnv; (* flush envs process created *)
if status = sleepqueue then deClkQueue(curInt);
if cm <> nil then
with cm↑ do
if oldcmon <> nil then
begin
with cmon↑.cdef↑ do ee := getVar(level,offset);
ee↑.c := oldcmon;
freePdb(pdb); (* done with this incarnation of cmon *)
if cmon↑.oncond↑.ntype = forcenode then freeEvent(evt);
relCmoncb(cm);
end
else
begin (* set us up for later *)
priority := (priority mod 10) + 1; (* base level priority again *)
spc := cm↑.cmon;
mode := 0;
status := nullqueue;
running := false;
enabled := false;
end
else
begin
if (not procp) and (evt <> nil) then freeEvent(evt);
freePdb(curInt);
end;
end;
end;
curInt := po;
end;
procedure flushAll(p: pdbp; dLev: integer); (* for use by EDIT *)
var b: boolean; i: integer; e: eventp; pp,po: pdbp;
begin
flushLevel(dLev);
if p <> nil then
begin (* flush process *)
po := curInt;
curInt := p;
with curInt↑ do
begin
killStack;
while level < getELev(env) do killEnv; (* flush envs process created *)
if status = sleepqueue then deClkQueue(curInt);
if cm = nil then relPdb(curInt);
end;
curInt := po;
end;
if dLev = 0 then
begin
etime := 0;
stime := 0;
curtime := 0;
curInt := nil;
activeInts := nil;
readQueue := nil;
resched := false;
(* *** would like to flush any leftover events, unless we saved outermost *** *)
(* *** environment - if we are then we can't.... *** *)
(* while allEvents <> nil do freeEvent(allEvents); (* flush any old events *)
e := allEvents; (* at least we can reset them though *)
while e <> nil do
with e↑ do
begin e↑.waitlist := nil; count := 0; e := next end;
curInt := getPdb;
speedfactor↑.s := 2.0; (* re-initialize speed_factor *)
iSingleThreadMode := false; (* reset no wait mode *)
(* ??? any other system defined variables need to be reset/reinitialized? ??? *)
end;
end;
procedure unwind(p: pdbp; eLev: integer); (* for use by EDIT *)
var po: pdbp;
begin
po := curInt;
curInt := p;
while eLev < getELev(curInt↑.env) do killEnv; (* unwind inner environments *)
curInt := po;
end;
procedure flushPdb(* p: pdbp *); (* for use by EDIT *)
var po: pdbp;
begin
if p↑.status = runqueue then
if activeInts = p then activeInts := p↑.next
else
begin
po := activeInts;
while (po↑.next <> nil) and (po↑.next <> p) do po := po↑.next;
if po <> nil then po↑.next := p↑.next;
end
else if p↑.status = inputqueue then
if readQueue = p then readQueue := p↑.next
else
begin
po := readQueue;
while (po↑.next <> nil) and (po↑.next <> p) do po := po↑.next;
if po <> nil then po↑.next := p↑.next;
end;
if (not p↑.procp) and (p↑.cm = nil) and (p↑.evt <> nil) then
if p↑.evt↑.count = -1 then
begin (* signal parent pdb *)
p↑.evt↑.waitlist↑.status := runqueue;
addPdb(activeInts,p↑.evt↑.waitlist); (* make parent active *)
freeEvent(p↑.evt);
end
else
begin
p↑.evt↑.count := p↑.evt↑.count + 1; (* other threads still executing *)
p↑.evt := nil; (* so flushLevel doesn't flush it *)
end;
p↑.priority := 255; (* so we can free just this process using flushLevel *)
flushLevel(25);
end;
procedure flushKids(p: pdbp; zapit: boolean);
var pp: pdbp; b: boolean;
begin
if p↑.status = joinwait then
begin
b := false;
repeat
pp := allPdbs;
repeat (* find one of the threads *)
with pp↑ do
if (not procp) and (cm = nil) and (evt <> nil) then
if evt↑.waitlist = p then
begin flushKids(pp,true); pp := nil end; (* flush it *)
if pp <> nil then (* move on to next *)
begin pp := pp↑.nextPdb; b := pp = nil end;
until pp = nil;
until b; (* repeat til we find all of them *)
end
else if p↑.status = proccall then
begin
pp := allPdbs;
repeat
if pp↑.procp and (pp↑.opdb = p) then
begin flushKids(pp,true); pp := nil end (* flush it *)
else pp := pp↑.nextPdb;
until pp = nil;
p↑.status := runqueue;
addPdb(activeInts,p);
end;
if zapit then flushPdb(p);
end;
(* aux routines: cmonEnable, cmonDisable, cmonCheck *)
procedure cmonEnable(e: enventryp);
var p: pdbp; b: boolean; pri: integer;
begin
with e↑.c↑ do
if (enabled or running) and ((pdb↑.priority mod 10) < debugLevel) then
makeCmon(e,cmon↑.cdef); (* push old & make another for this debug level *)
with e↑.c↑ do
begin
pdb↑.mech := curInt↑.mech; (* inherit device being controlled *)
if running then enabled := true (* if currently running, re-enable it *)
else if not enabled then (* is it currently enabled? *)
begin
enabled := true; (* now it is *)
pdb↑.status := runqueue;
pdb↑.priority := (pdb↑.priority mod 10) + (10 * debuglevel);
addPdb(activeInts,pdb); (* add cmon to list of active processes *)
if pdb↑.priority > curInt↑.priority then
resched := true; (* need to swap us out *)
end;
end;
end;
procedure cmonDisable(c: cmoncbp);
var p,pp: pdbp; b: boolean; n,np: nodep;
begin
with c↑ do
begin
if enabled then (* is it currently enabled? *)
begin
enabled := false; (* disable it *)
if cmon↑.oncond↑.ntype = forcenode then
begin
with msg↑ do
begin
cmd := forceoffcmd;
bits := fbits;
evt := c↑.evt;
end;
sendCmd; (* tell force system to stop checking for this force condition *)
end;
if cmon↑.exprCm or (cmon↑.oncond↑.ntype = durnode) then deClkQueue(pdb)
else
begin (* remove pdb from event queue *)
p := evt↑.waitlist;
pp := nil;
while (p <> nil) and (p <> pdb) do begin pp := p; p := p↑.next end;
if p <> nil then (* found us, now splice us out of the list *)
if pp = nil then evt↑.waitlist := p↑.next else pp↑.next := p↑.next;
end;
pdb↑.next := nil;
end;
end;
end;
function cmonCheck: boolean;
var b: boolean; i: integer; env: environp; ev: enventryp;
begin (* make sure all cmon's in current environment have finished *)
b := true;
env := curInt↑.env↑.env[0]; (* point to first environment record *)
i := 0;
ev := env↑.vals[0];
while (ev <> nil) and b do
with ev↑ do
begin (* see if any cmons are running *)
if etype = cmontype then
begin (* found a cmon *)
if c↑.running then
b := c↑.pdb↑.priority >= curInt↑.priority (* is it running now? *)
else cmonDisable(c); (* if not disabled it *)
end;
i := i + 1;
if i <= 9 then ev := env↑.vals[i]
else
begin
i := 0;
env := env↑.next; (* use next env record *)
if env <> nil then ev := env↑.vals[0] else ev := nil;
end;
end;
cmonCheck := b; (* true if no cmons are now running *)
end;
(* expression evaluator: evalExp *)
procedure evalExp;
var res, n1, n2, n3: nodep; p: pdbp; i, j, tbits: integer; vfp: varidefp;
ep,epar: enventryp; envir: environp; envhdr: envheaderp; ch: ascii;
b, b1, b2, b3: boolean;
begin
with curInt↑.epc↑ do
begin
if ntype = leafnode then
if ltype = varitype then with vari↑ do getVal(level, offset)
else begin (* should only get here for constants, badops & subscripts *)
if ltype = pconstype then n1 := pcval else n1 := curInt↑.epc;
res:= newNode;
with res↑ do
begin
ntype := leafnode;
ltype := n1↑.ltype;
length := n1↑.length; (* this should work for all leaftypes *)
str := n1↑.str;
end;
push(res);
end
else if ntype = exprnode then
begin
n2 := nil; b2 := false;
n3 := nil; b3 := false;
if (op < ioop) or (op = adcop) or (op = dacop) then (* not a special op *)
begin (* pop appropriate number of args off of stack *)
n1 := getNval(arg1,b1); (* all ops have at least one arg *)
if arg2 <> nil then
begin
n2 := getNval(arg2,b2);
if arg3 <> nil then
begin
n3 := getNval(arg3,b3);
end;
end
end
else begin n1 := nil; b1 := false end;
if (op < specop) or (op = adcop) or (op = jointop) then
begin (* if it's not a special op *)
res := newNode;
res↑.ntype := leafnode;
if (op < vecop) or (ioop < op) then res↑.ltype := svaltype
else if op < transop then res↑.ltype := vectype
else res↑.ltype := transtype;
end;
case op of (* assumes correct args on stack *)
(* relations *)
sltop: if n1↑.s < n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sleop: if n1↑.s <= n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
seqop: if n1↑.s = n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sgeop: if n1↑.s >= n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sgtop: if n1↑.s > n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
sneop: if n1↑.s <> n2↑.s then res↑.s := 1.0 else res↑.s := 0.0;
(* logical *)
notop: if n1↑.s = 0.0 then res↑.s := 1.0 else res↑.s := 0.0;
orop: if (n1↑.s <> 0) or (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
xorop: if (n1↑.s <> 0) <> (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
andop: if (n1↑.s <> 0) and (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
eqvop: if (n1↑.s <> 0) = (n2↑.s <> 0) then res↑.s := 1.0 else res↑.s := 0.0;
(* scalar ops *)
saddop: res↑.s := n1↑.s + n2↑.s;
ssubop: res↑.s := n1↑.s - n2↑.s;
smulop: res↑.s := n1↑.s * n2↑.s;
sdivop: res↑.s := n1↑.s / n2↑.s;
snegop: res↑.s := - n1↑.s;
sabsop: res↑.s := abs(n1↑.s);
sexpop: res↑.s := exp(n2↑.s * ln(n1↑.s));
maxop: if n1↑.s > n2↑.s then res↑.s := n1↑.s else res↑.s := n2↑.s;
minop: if n1↑.s < n2↑.s then res↑.s := n1↑.s else res↑.s := n2↑.s;
intop: res↑.s := round(n1↑.s);
idivop: res↑.s := round(n1↑.s) div round(n2↑.s);
modop: res↑.s := round(n1↑.s) mod round(n2↑.s);
(* functions *)
sqrtop: res↑.s := sqrt(n1↑.s);
logop: res↑.s := ln(n1↑.s);
expop: res↑.s := exp(n1↑.s);
timeop: res↑.s := curtime - n1↑.s; (* ** daytime? conversion to secs? ** *)
(* trig *)
sinop: res↑.s := sind(n1↑.s);
cosop: res↑.s := cosd(n1↑.s);
tanop: res↑.s := tand(n1↑.s);
asinop: res↑.s := asin(n1↑.s);
acosop: res↑.s := acos(n1↑.s);
atan2op: res↑.s := atan2(n1↑.s,n2↑.s);
(* vector ops *)
vdotop: res↑.s := vdot(n1↑.v,n2↑.v);
vmagnop: res↑.s := vmagn(n1↑.v);
unitvop: res↑.v := unitv(n1↑.v);
vaddop: res↑.v := vadd(n1↑.v,n2↑.v);
vsubop: res↑.v := vsub(n1↑.v,n2↑.v);
vnegop: res↑.v := svmul(-1.0,n1↑.v);
crossvop: res↑.v := vcross(n1↑.v,n2↑.v);
vmakeop: res↑.v := vmake(n1↑.s,n2↑.s,n3↑.s);
svmulop: res↑.v := svmul(n1↑.s,n2↑.v);
vsmulop: res↑.v := svmul(n2↑.s,n1↑.v);
vsdivop: res↑.v := vsdiv(n1↑.v,n2↑.s);
tvmulop: res↑.v := tvmul(n1↑.t,n2↑.v);
wrtop: res↑.v := tvmul(torient(n2↑.t),n1↑.v);
(* trans ops *)
tposop: res↑.v := tpos(n1↑.t);
taxisop: res↑.v := taxis(n1↑.t);
tmagnop: res↑.s := tmagn(n1↑.t);
fmakeop,
tmakeop: res↑.t := tmake(n1↑.t,n2↑.v);
torientop: res↑.t := torient(n1↑.t);
ttmulop: res↑.t := ttmul(n1↑.t,n2↑.t);
tvaddop: res↑.t := tvadd(n1↑.t,n2↑.v);
tvsubop: res↑.t := tvsub(n1↑.t,n2↑.v);
tinvrtop: res↑.t := tinvrt(n1↑.t);
vsaxwrop: res↑.t := vsaxwr(n1↑.v,n2↑.s);
constrop: res↑.t := construct(n1↑.v,n2↑.v,n3↑.v);
ftofop: res↑.t := ttmul(tinvrt(n1↑.t),n2↑.t);
vmkfrcop: res↑.t := vmkfrc(n1↑.v);
(* input ops *)
queryop: begin (* now print everything out *)
b := false;
if not inputReady then
if readQueue = nil then
begin (* first time through *)
prntplist(arg2);
b := true;
end
else if (readQueue↑.priority div 10) < (curInt↑.priority div 10) then
begin (* first time through *)
prntplist(arg2);
b := true;
end
else sleep(60) (* wait a sec for other input to finish *)
else
begin
inputReady := false;
ch := inputLine[1];
if ord(ch) >= smallA then
ch := chr(ord(ch) - ord(' ')); (* make upper case *)
if (ch = 'Y') or (ch = 'N') then
begin
if ch = 'Y' then res↑.s := 1.0 else res↑.s := 0.0;
push(res);
end
else b := true; (* ask again *)
end;
if b then
begin
relNode(res);
pp20L('Type Y or N: ',13);
ppOutNow;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end
end;
inscalarop: begin
if not inputReady then
begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10)<(curInt↑.priority div 10);
if b then
begin (* first time through *)
pp20L('Scalar please: ',15); ppOutNow;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end
else sleep(60); (* wait a sec for other input to finish *)
relNode(res);
end
else
begin (* parse the number *)
inputReady := false;
b := true; (* assume plus *)
i := 1;
while (i <= inputp) and (inputLine[i] = ' ') do i := i + 1;
if inputLine[i] = '+' then i := i + 1
else if inputLine[i] = '-' then begin b := false; i := i + 1 end;
while (i <= inputp) and (inputLine[i] = ' ') do i := i + 1;
j := 0;
while (i <= inputp) and (* get integer part *)
('0' <= inputLine[i]) and (inputLine[i] <= '9') do
begin j := 10*j + ord(inputLine[i]) - ord('0'); i := i + 1 end;
res↑.s := j;
if inputLine[i] = '.' then
begin (* get fractional part *)
i := i + 1;
j := 10;
while (i <= inputp) and
('0' <= inputLine[i]) and (inputLine[i] <= '9') do
begin
res↑.s := res↑.s + (ord(inputLine[i]) - ord('0')) / j;
j := 10 * j;
i := i + 1;
end;
end;
if not b then res↑.s := - res↑.s;
push(res);
end;
end;
vmop: ;
adcop: with msg↑ do
begin
cmd := readadccmd;
n := round(n1↑.s); (* get channel # *)
if (n < 0) or (63 < n) then (* bad channel # *)
begin
pp20L('A/D channel out of r',20); pp20('ange - using chan 0 ',19);
ppLine;
n := 0;
end;
getReply(true); (* have ARM servo read it in *)
res↑.s := val; (* store result away *)
end;
dacop: with msg↑ do
begin
cmd := writedaccmd;
n := round(n1↑.s); (* get channel # *)
if (n < 1) or (4 < n) then (* bad channel # *)
begin
pp20L('D/A channel out of r',20); pp20('ange - using chan 1 ',19);
ppLine;
n := 1;
end;
val := n2↑.s; (* & magnitude *)
sendCmd; (* have ARM servo write it out *)
end;
jointop: with msg↑ do
begin
cmd := wherecmd;
bits := joint1cb+joint2cb+joint3cb+joint4cb+joint5cb+joint6cb;
with arg1↑.vari↑ do
ep := getVar(level,offset); (* environment entry for device *)
dev := ep↑.f↑.mech; (* don't need to check it's a valid device *)
getReply(true); (* have ARM servo read it in *)
n1 := getNval(arg2↑.lval,b1); (* now see which joint is wanted *)
i := round(n1↑.s); (* get joint # *)
if (i < 0) or (6 < i) then (* bad joint # *)
begin
pp20L('Joint number out of ',20); pp20('range - using jt 1 ',18);
ppLine;
i := 1;
end;
if version = 10 then res↑.s := 90.0 (* for 10 version *)
else if ok then res↑.s := t[i] (* fetch & store result away *)
else
begin (* ERROR - complain *)
ppArmError(error,bits);
res↑.s := 0;
end;
end;
(* special *)
arefop: with arg1↑.vari↑ do getVal(level,offset); (* should never get here *)
callop: begin
p := getPdb;
with p↑ do
begin
opdb := curInt;
procp := true;
status := nowrunning;
pdef := arg1↑.vari↑.p;
level := pdef↑.level;
spc := pdef↑.body; (* code to execute *)
end;
with arg1↑.vari↑ do
ep := getVar(level, offset); (* environment entry for procedure *)
envhdr := newEheader;
p↑.env := envhdr;
with envhdr↑ do
begin
parent := ep↑.penv; (* parent is env where proc defined *)
procp := true;
proc := ep↑.p;
varcnt := 0;
for j := 1 to 4 do env[j] := nil;
end;
vfp := ep↑.p↑.paramlist; (* formal parameters *)
n1 := arg2; (* actual parameters *)
envir := newEnvironment; (* always need at least one environment record *)
envir↑.next := nil;
envhdr↑.env[0] := envir;
for j := 0 to 9 do envir↑.vals[j] := nil;
i := 0;
j := -1;
while vfp <> nil do (* make parameter variables *)
begin
epar := enterEntry(i,j,envir,envhdr,vfp);
tbits := vfp↑.tbits;
if tbits = 4 then (* call by reference *)
with n1↑.lval↑ do
if ((ntype = exprnode) and (op <> arefop)) or (* expression *)
((ntype = leafnode) and (ltype <> varitype)) (* constant *)
then tbits := 0; (* change to call by value *)
makeVar(epar,vfp,tbits); (* make var's environment entry *)
with n1↑.lval↑ do (* now bind actual parameter value *)
if tbits = 5 then (* array passed by reference *)
with vari↑ do epar↑.r := getEntry(level,offset)
else if tbits = 4 then (* regular variable passed by reference *)
epar↑.r := gtVarn(n1↑.lval)
else (* need to copy value *)
begin
n2 := getNval(n1↑.lval,b);
with epar↑ do
case etype of
svaltype: s := n2↑.s;
vectype,
transtype: begin
v := n2↑.v;
v↑.refcnt := v↑.refcnt + 1;
end;
frametype: begin
f↑.val := n2↑.t;
f↑.valid := 0; (* mark us as valid *)
f↑.val↑.refcnt := f↑.val↑.refcnt + 1;
end;
strngtype: begin length := n2↑.length; str := n2↑.str end;
end;
if b then killNode(n2); (* done with stack entry *)
end;
n1 := n1↑.next;
vfp := vfp↑.next;
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
curInt↑.epc := curInt↑.epc↑.next; (* advance our epc now *)
curInt↑.status := proccall;
curInt := p; (* swap to procedure now *)
end;
badop: ;
end;
if (op < ioop) or (op = adcop) or (op = jointop) then
push(res); (* save result on stack *)
if b1 then relNode(n1); (* release nodes when done with them *)
if b2 then relNode(n2);
if b3 then relNode(n3);
end
else if ntype <> listnode then
begin (* **** error - bad node **** *)
pp20L('Error in Eval - bad ',20); pp10('node type ',9); ppLine;
(* code to recover??? *)
end;
end;
if curInt <> nil then (* in case we're now waiting for input *)
with curInt↑ do (* advance pointer to next node to be evaluated *)
if epc <> nil then epc := epc↑.next;
end;
procedure doProg; (* ** ** *)
begin
(* *** stuff to reset affixments *** *)
speedfactor↑.s := 2.0; (* initialize speed_factor *)
if version = 10 then garm↑.tdest := gpark; (* for 10 version *)
curInt↑.spc := curInt↑.spc↑.pcode;
curInt↑.mode := 0;
end;
procedure doBlock;
var i,j: integer; v: varidefp;
envhdr: envheaderp; e: enventryp; envir: environp;
begin
with curInt↑ do
begin
if spc↑.variables <> nil then
with spc↑ do
begin
envhdr := newEheader;
envhdr↑.parent := env;
env := envhdr;
envhdr↑.block := spc;
envhdr↑.varcnt := 0;
envhdr↑.procp := false;
envir := newEnvironment; (* always need at least one environment record *)
envir↑.next := nil;
envhdr↑.env[0] := envir;
for j := 1 to 4 do envhdr↑.env[j] := nil;
for j := 0 to 9 do envir↑.vals[j] := nil;
i := 0;
j := -1;
v := variables;
while v <> nil do
begin
if v↑.vtype < dimensiontype then
begin
e := enterEntry(i,j,envir,envhdr,v);
makeVar(e,v,v↑.tbits); (* make variable environment entry *)
end
else (* if v↑.vtype = freevartype then - need to do it for macros too *)
begin
relEentry(enterEntry(i,j,envir,envhdr,v)); (* space past env entry *)
envir↑.vals[j] := nil;
end;
v := v↑.next
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
end;
mode := 0;
spc := spc↑.bcode;
end;
end;
procedure doCoblock;
var e: eventp;
procedure sched(n: nodep);
var p: pdbp;
begin
if n↑.next <> nil then sched(n↑.next); (* maintain lexical order *)
if n↑.cstmnt↑.stype <> commenttype then
begin (* we don't want to schedule comments (yet) *)
p := getPdb; (* get a pdb for this thread *)
with p↑ do
begin
next := activeInts; (* add us to list of active interpreters *)
activeInts := p;
status := runqueue;
spc := n↑.cstmnt;
sdef := spc;
evt := e; (* event to signal when we go away *)
end;
end;
end;
begin
with curInt↑ do
case mode of
1: begin (* schedule the parallel threads for execution *)
mode := 2;
if spc↑.threads <> nil then
begin
e := getEvent; (* event to use for signalling when all threads are done *)
e↑.count := -spc↑.nthreads;
e↑.waitlist := curInt;
sched(spc↑.threads); (* schedule all the threads *)
curInt↑.status := joinwait;
curInt := nil;
resched := true; (* start up first of them *)
end;
end;
2: begin (* all threads are done - continue with main *)
mode := 0;
spc := spc↑.next;
end;
end;
end;
procedure doEnd;
var spcp: statementp; e: eventp; b: boolean; n: nodep;
begin
b := true;
with curInt↑ do
begin
spcp := spc↑.bparent;
mode := 0; (* assume this *)
case spcp↑.stype of
progtype: begin
running := false; (* all done running *)
end;
blocktype: begin
if spcp↑.variables <> nil then (* any variables? *)
b := cmonCheck; (* any cmons now running? *)
if b then
begin (* no - we can clean things up *)
if spcp↑.variables <> nil then killEnv;
spcp := spcp↑.next;
end
else sleep(30); (* give cmons time to finish *)
end;
coblocktype: begin
if evt = nil then
begin
running := false; (* break to debugger *)
(* *** if not iSingleThreadMode then complain??? *** *)
end
else
begin
b := false;
e := evt;
killStack; (* flush stack *)
freePdb(curInt);
if e↑.count = -1 then
begin (* this was last thread *)
curInt := e↑.waitlist; (* return to main *)
curInt↑.status := nowrunning;
freeEvent(e);
if activeInts <> nil then
if curInt↑.priority < activeInts↑.priority then
resched := true;
end
else
begin (* other threads still executing *)
e↑.count := e↑.count + 1;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
end;
cmtype: begin (* terminate or resched this cmon *);
cm↑.running := false;
killStack;
if not cm↑.enabled then
begin (* we're done, swap us out *)
b := false;
spc := spcp; (* set us up for next time *)
curInt↑.status := nullqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
fortype: begin
n := sp; (* first try to find forvalnode on stack *)
b := false;
while (n <> nil) and (not b) do
begin
b := sp↑.ntype = forvalnode; (* a FOR node? *)
if b then b := spcp = sp↑.fstmnt; (* right one? *)
if not b then n := n↑.next; (* no, try next *)
end;
if b then
begin
while sp <> n do killNode(pop); (* flush any extra stack nodes *)
sp↑.fvar↑.s := sp↑.fvar↑.s + sp↑.fstep; (* next FOR value *)
mode := 2; (* do FOR check *)
end
else
begin (* Gack!!! Stack Error *)
pp20L('Can''t find FOR node ',20); pp20('- stack error!!! ',16);
ppLine;
(* could try to recover, but.... just abort FOR loop *)
spc := spcp↑.next;
end;
end;
untiltype: mode := 2;
whiletype: (* nothing to do *);
movetype, (* for error handler *)
iftype,
casetype: begin
spcp := spcp↑.next;
end;
end;
if b then spc := spcp;
end;
end;
procedure doFor;
var ev: enventryp; fnode, res: nodep;
begin
with curInt↑ do
case mode of
1: begin (* stack contains: forvar subscripts, initial, step & final values *)
ev := gtVarn(spc↑.forvar); (* access variable *)
res := pop; (* get initial value *)
ev↑.s := res↑.s; (* store it away *)
relNode(res); (* release node *)
fnode := sp; (* get step value *)
fnode↑.ntype := forvalnode;
fnode↑.fstep := fnode↑.s; (* copy step value - note s & step fields may overlap *)
fnode↑.fvar := ev; (* copy environment entry *)
fnode↑.fstmnt := spc; (* pointer to FOR statement *)
mode := 2;
end;
2: begin
fnode := sp;
if (fnode↑.fvar↑.s - fnode↑.next↑.s) * fnode↑.fstep <= 0.0 (* (cur-fin)*step *)
then spc:= spc↑.fbody (* go interpret for body *)
else begin
spc := spc↑.next; (* move on to next statement *);
res := fnode↑.next;
sp := res↑.next; (* pop FOR nodes off of stack *)
relNode(fnode); (* and release them *)
relNode(res);
end;
mode := 0;
end;
end;
end;
procedure doIf;
var res: nodep; s: statementp;
begin
with curInt↑ do
begin
res := pop; (* pop value off of stack *)
s := spc;
if res↑.s = 0.0 then spc := s↑.els else spc := s↑.thn;
if spc = nil then spc := s↑.next; (* if nil clause just go on to next stmnt *)
relNode(res);
mode := 0;
end;
end;
procedure doWhile;
var res: nodep;
begin
with curInt↑ do
begin
res := pop; (* pop value off of stack *)
if res↑.s = 0.0 then spc := spc↑.next (* if false move on to next stmnt *)
else if spc↑.body <> nil then spc := spc↑.body;
relNode(res);
mode := 0;
end;
end;
procedure doUntil;
var res: nodep;
begin
with curInt↑ do
case mode of
1: begin
if spc↑.body <> nil then begin spc := spc↑.body; mode := 0 end
else mode := 2;
end;
2: begin
epc := spc↑.exprs; (* need to evaluate until condition *)
mode := 3;
end;
3: begin
res := pop; (* pop value off of stack *)
if (res↑.s <> 0.0) then
begin
spc := spc↑.next; (* if true move on to next stmnt *)
mode := 0;
end
else mode := 1; (* if still false repeat body *)
relNode(res);
end;
end;
end;
procedure doCase;
var i: integer; p: nodep; spcp: statementp; b: boolean;
begin
with curInt↑ do
begin
p := pop; (* pop index value off of stack *)
i := round(p↑.s);
relNode(p);
spcp := nil;
p := spc↑.caselist;
if (i >= 0) and (i <= abs(spc↑.range)) then (* index within range *)
begin (* try to find proper case *)
b := true;
while (p <> nil) and b do
if (p↑.cval = i) then b := false else p := p↑.next;
if p <> nil then
begin spcp := p↑.stmnt; if spcp = nil then spcp := spc↑.next end
else if spc↑.range >= 0 then spcp := spc↑.next (* null statement *)
end;
if (spcp = nil) and (spc↑.range < 0) then
begin (* if none found and it's a labelled case statement check for else *)
p := spc↑.caselist;
b := true;
while (p <> nil) and b do (* search for else stmnt *)
if (p↑.cval = -1) then b := false else p := p↑.next;
if p <> nil then spcp := p↑.stmnt
end;
if spcp = nil then
begin
pp20L('Case index out of ra',20); pp5('nge: ',5); ppInt(i); ppLine;
spcp := spc↑.next;
end;
spc := spcp;
mode := 0;
end;
end;
procedure doCall;
var n: nodep;
begin
with curInt↑ do
begin
if spc↑.what↑.arg1↑.vari↑.vtype <> nulltype then (* flush unused result *)
n := pop;
mode := 0;
spc := spc↑.next; (* move on to next statement *);
end;
end;
procedure doReturn;
var p: pdbp; n: nodep; b,debRet: boolean;
begin
b := true;
with curInt↑ do
begin
if procp then debRet := false (* normal case *)
else if (priority > 9) and (nextpdb = nil) and (opdb <> nil) then
debRet := true (* immediately executed RETURN *)
else b := false; (* no good - nothing to return from *)
if debRet then p := opdb↑.opdb else p := opdb; (* pdb of caller *)
if b then
begin
while b and not env↑.procp do
begin (* make sure all cmon's in outer environments have finished *)
b := cmonCheck;
if b then killEnv; (* flush all environments out to parameters *)
end;
if b then (* no cmons now running *)
begin (* now we can clean things up & return from the procedure *)
if spc↑.retval <> nil then n := pop (* get return value *)
else n := nil;
if env↑.proc↑.ptype <> nulltype then
begin (* yes - put return value on caller's stack *)
if n <> nil then
if env↑.proc↑.ptype <> n↑.ltype then
begin
killNode(n);
n := nil;
end;
if n = nil then
begin
n := newNode;
with n↑ do (* use default value *)
begin
ntype := leafnode;
ltype := env↑.proc↑.ptype; (* copy datatype of result *)
if ltype = svaltype then s := 0.0 (* it's a scalar *)
else if ltype = vectype then v := nilvect
else if ltype = transtype then t := niltrans
else begin length := 0; str := nil end;
end;
end;
n↑.next := p↑.sp;
p↑.sp := n;
end;
killEnv; (* flush procedure's parameters too *)
killStack; (* flush stack *)
if debRet then
begin
opdb↑.opdb↑.status := runqueue;
addPdb(activeInts,opdb↑.opdb); (* re-activate caller *)
opdb↑.level := 255; (* so we don't re-release environments *)
flushKids(opdb,true); (* flush old procedure's pdb *)
spc := sdef↑.next; (* point to our abort *)
running := false; (* and return to debugger *)
end
else
begin
freePdb(curInt); (* flush procedure's pdb *)
curInt := p; (* all done - return *)
curInt↑.status := nowrunning;
end;
end
else sleep(30); (* give cmons time to finish *)
end
else
begin
pp20L('Ignoring return ',16); ppLine;
if spc↑.retval <> nil then n := pop; (* flush return value *)
spc := spc↑.next; (* just move on to next statement *)
mode := 0;
end;
end;
end;
procedure doPrint;
begin
with curInt↑ do
begin (* print everything out *)
prntplist(spc↑.plist);
mode := 0;
spc := spc↑.next;
end;
end;
procedure doPrompt;
const smallP = 112; (* Lowercase p *)
var ch: ascii; b: boolean;
begin
with curInt↑ do
case mode of
1: begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10) < (curInt↑.priority div 10);
if b then
begin (* first time through *)
prntplist(spc↑.plist);
mode := 2;
end
else sleep(60) (* wait a sec for other input to finish *)
end;
2: begin
pp20L('Type P to proceed: ',19);
ppOutNow;
mode := 3;
curInt↑.next := readQueue;
readQueue := curInt; (* swap us out *)
curInt↑.status := inputqueue;
curInt := nil;
inputp := 0;
resched := true;
end;
3: begin
inputReady := false;
if (inputLine[1] = chr(smallP)) or (inputLine[1] = 'P') then
begin
mode := 0;
spc := spc↑.next;
end
else mode := 2; (* try again *)
end;
end;
end;
procedure doPause;
var i: integer; n: nodep;
begin
n := pop;
i := round(n↑.s * 60); (* get pause time (in 60Hz ticks) *)
relNode(n);
curInt↑.mode := 0; (* get ready for next statement *)
curInt↑.spc := curInt↑.spc↑.next;
sleep(i); (* put us to sleep for a while *)
end;
procedure doAbort;
begin
with curInt↑ do
begin (* print everything out *)
if spc↑.debugLev = 0 then
begin (* a real abort *)
(* tell arm servo to abort all motions in progress *)
(*
{$C .MCALL SETF$S
SETF$S #40. ;Signal Aborts by setting common event flag
}
*)
(* msg↑.cmd := abortcmd; *) (* latter we'll do it with messages *)
(* sendCmd; *)
prntplist(spc↑.plist);
spc := spc↑.next;
pp10L('Aborting ',8);
running := false; (* break to debugger *)
end
else if debugLevel = spc↑.debugLev then
running := false (* break if debugger process *)
else spc := spc↑.next; (* just ignore it *)
mode := 0;
end;
end;
procedure doSay;
var n,np: nodep; b: boolean;
procedure sayInt(i: integer);
var j: integer; n: array [1..9] of integer;
begin
for j := 1 to 9 do (* get individual digits *)
begin n[j] := i mod 10; i := i div 10 end;
j := 9;
while (j > 1) and (n[j] = 0) do j := j - 1; (* ignore leading zeros *)
for i := j downto 1 do
if version = 11 then write(talk,chr(ord('0')+n[i])) (* say digit *)
else ppChar(chr(ord('0')+n[i])); (* print it *)
end;
procedure saySval(s: real);
var si: real; ip,fp: integer;
begin
if s < maxInt then
begin
si := trunc(s);
s := si + round(1000*(s-si))/1000;
ip := trunc(s);
fp := trunc(1000*(s-ip));
sayInt(ip); (* say integer part *)
if fp > 0 then
begin (* say fractional part too *)
if version = 11 then write(talk,' point ')
else pp10(' point ',7);
sayInt(fp);
end;
end
else
begin (* it's a bignum *)
fp := 0;
repeat fp := fp + 1; s := s / 10 until s <= maxint; (* scale it down *)
sayInt(trunc(s)); (* say significant digits *)
for ip := 1 to fp do (* now the trailing zeros *)
if version = 11 then write(talk,'0') else ppChar('0');
end;
if version = 11 then write(talk,' ,, ') (* add a small pause *)
else ppChar(' ');
end;
procedure sayVec(v: vectorp);
var i: integer;
begin
if version = 11 then write(talk,' vector ')
else pp10('vector ',7);
with v↑ do
for i := 1 to 3 do
begin
saySval(val[i]);
end;
if version = 11 then write(talk,' ,, '); (* add a small pause *)
end;
procedure sayTrans(t: transp);
var i: integer; v: vectorp;
begin
with t↑ do
begin
refcnt := refcnt + 1;
if version = 11 then write(talk,' trans rot ')
else pp10('trans rot ',10);
v := taxis(t); sayVec(v); relVector(v);
saySval(tmagn(t));
if version = 11 then write(talk,' , vector ')
else pp10(' , vector ',10);
for i := 1 to 3 do
begin
saySval(val[i,4]);
end;
if version = 11 then write(talk,' ,, '); (* add a small pause *)
refcnt := refcnt - 1;
end;
end;
procedure sayStrng(length: integer; s: strngp);
var i,j: integer; cntl: boolean; ch: ascii;
begin
j := 1;
cntl := false;
for i := 1 to length do
begin
ch := s↑.ch[j];
if cntl then
begin (* make it a control char *)
if ord(ch) >= smallA then
ch := chr(ord(ch) - ord(' ')); (* convert to uppercase *)
if version = 11 then write(talk,chr(ord(ch) - ord('@')))
else begin ppChar('↑'); ppChar(ch) end;
cntl := false;
end
else if ch = '\' then cntl := true
else
if version = 11 then write(talk,ch) else ppChar(ch);
if j = 10 then begin j := 1; s := s↑.next; end
else j := j + 1;
end;
end;
begin
with curInt↑ do
begin (* say whatever user wants us to *)
n := spc↑.plist;
if version = 10 then
if n <> nil then pp10l('Speaking: ',10);
while n <> nil do (* say everything on the list *)
begin
np := getNval(n↑.lval,b);
if np <> nil then
begin
with np↑ do
case ltype of
svaltype: saySval(s);
vectype: sayVec(v);
transtype: sayTrans(t);
strngtype: sayStrng(length,str);
end;
if b then killNode(np); (* flush used stack entry *)
end;
n := n↑.next;
end;
if spc↑.plist <> nil then
begin
if version = 11 then
begin writeln(talk); break(talk) end (* say it now *)
else ppLine;
end;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doAssign;
var ev: enventryp; res: nodep;
begin
with curInt↑.spc↑.what↑ do
begin
if ntype = leafnode then
with vari↑ do setVal(level,offset) (* store into simple variable *)
else
case op of (* see what type of store we're to do *)
arefop: with arg1↑.vari↑ do setVal(level,offset); (* store into array var *)
deproachop: begin (* any subscripts & deproach value on stack *)
ev := gtVarn(curInt↑.spc↑.what); (* access variable *)
res := pop; (* get deproach value *)
(* check we've really got a frame? *)
ev↑.f↑.fdepr := res↑.t; (* store it away *)
relNode(res);
end;
tposop,
torientop: begin
with arg1↑ do
if ntype = leafnode then
with vari↑ do setVal(level,offset) (* simple variable *)
else
with arg1↑.vari↑ do setVal(level,offset); (* array variable *)
end;
end;
curInt↑.mode := 0;
curInt↑.spc := curInt↑.spc↑.next; (* move on to next statement *);
end;
end;
procedure doSignal;
var ev: enventryp; p, pt: pdbp; st: statementp;
begin
with curInt↑ do
begin
st := spc;
spc := spc↑.next; (* advance our pc now before possibly swapping ourself out *)
mode := 0;
if iSingleThreadMode then
begin
pp20L('Would signal event: ',20); prntVar(st↑.event);
end
else if st↑.event <> nil then
begin
ev := gtVarn(st↑.event); (* access variable *)
ev↑.evt↑.count := ev↑.evt↑.count + 1;
p := ev↑.evt↑.waitlist; (* get pdb of process to schedule (if any) *)
if p <> nil then
begin
ev↑.evt↑.waitlist := p↑.next; (* remove node from waitlist *)
if p↑.priority > priority then
begin (* swap it in and swap us out *)
p↑.status := nowrunning;
pt := curInt;
curInt := p;
p := pt;
end;
p↑.status := runqueue;
addPdb(activeInts,p); (* add whoever to active process list *)
end;
end;
end;
end;
procedure doWait;
var ev: enventryp; p: pdbp; st: statementp; b: boolean;
begin
with curInt↑ do
if iSingleThreadMode then
if mode = 1 then
begin
if readQueue = nil then b := true
else b := (readQueue↑.priority div 10) < (curInt↑.priority div 10);
if b then
begin (* first time through *)
pp20L('Would wait for event',20); pp5(': ',2); prntVar(spc↑.event);
mode := 2;
doPrompt; (* now have user type a "P" to proceed *)
end
else sleep(60) (* wait a sec for other input to finish *)
end
else doPrompt
else
begin
st := spc;
spc := spc↑.next; (* advance our pc now before maybe swapping out *)
mode := 0;
if st↑.event <> nil then
begin
ev := gtVarn(st↑.event); (* access variable *)
ev↑.evt↑.count := ev↑.evt↑.count - 1;
if ev↑.evt↑.count < 0 then (* hasn't been signalled yet, need to wait *)
begin
curInt↑.status := eventqueue;
addPdb(ev↑.evt↑.waitlist,curInt); (* add us to wait list *)
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
end;
end;
procedure doEnable;
begin
with curInt↑ do
begin
if spc↑.cmonlab = nil then
if cm <> nil then cm↑.enabled := true (* re-enabling this cmon *)
else
begin
pp20L('No cmon to enable! ',18); ppLine;
end
else
begin
with spc↑.cmonlab↑.s↑.cdef↑ do
cmonEnable(getVar(level,offset)); (* enable cmon control block *)
end;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doDisable;
var e: enventryp;
begin
with curInt↑ do
begin
if spc↑.cmonlab = nil then
if cm <> nil then cm↑.enabled := false (* disabling this cmon *)
else
begin
pp20L('No cmon to disable! ',19); ppLine;
end
else
begin
with spc↑.cmonlab↑.s↑.cdef↑ do
e := getVar(level,offset); (* get cmon control block *)
if e↑.c↑.running then sleep(30) (* if running wait for it to finish *)
else
begin
cmonDisable(e↑.c); (* disable it *)
mode := 0;
spc := spc↑.next;
end;
end;
end;
end;
(* affixment auxiliary routines: affixaux, unfixaux & unfix *)
procedure affixaux (f, d: framep; cnt: integer);
var c1,c2,ct: nodep;
begin
with f↑ do
if not (ftype and (dev <> nil)) then (* haven't marked it yet *)
begin
if not ftype then cnt := 1 (* it's a device *)
else begin dev := d; dcntr := cnt; cnt := cnt + 1; end; (* mark frame *)
c1 := calcs;
ct := nil;
while c1 <> nil do
begin (* mark everyone it's affixed to *)
if c1↑.rigid or not c1↑.frame1 then affixaux(c1↑.other,d,cnt)
else if c1↑.other↑.dev = nil then
begin (* need to break non-rigid affixment *)
(* first splice calcs out of affixment lists *)
if ct = nil then calcs := c1↑.next else ct↑.next := c1↑.next;
c2 := c1↑.other↑.calcs;
ct := nil;
while c2↑.other <> f do begin ct := c2; c2 := c2↑.next; end;
if ct = nil then c1↑.other↑.calcs := c2↑.next else ct↑.next := c2↑.next;
if not c1↑.tvarp then
begin (* release relation trans *)
upTrans(c1↑.tval,nil);
upTrans(c2↑.tval,nil);
end;
relNode(c1); (* finally release calc nodes *)
relNode(c2);
c1 := ct;
end;
ct := c1;
c1 := c1↑.next;
end;
end;
end;
function unfixaux (f: framep; cnt: integer): boolean;
var c: nodep; b: boolean; d: framep;
begin
b := false;
with f↑ do
if not ftype then affixaux(f,f,1) (* a device - remark everyone as dynamic *)
else if dev <> nil then (* check we're still marked as dynamic, else done *)
if cnt > dcntr then
begin
d := dev; dev := nil; (* so affixaux will mark us *)
affixaux(f,d,dcntr); (* need to remark everyone *)
end
else
begin (* unmark us *)
dev := nil;
dcntr := 0;
b := true;
c := calcs;
while (c <> nil) and b do
begin
b := unfixaux(c↑.other,cnt);
c := c↑.next
end
end;
unfixaux := b;
end;
procedure unfix (* f1,f2: framep *);
var t: transp; c1, c2: nodep; b: boolean; i: integer;
begin
if f1↑.ftype then t := feval(f1); (* try to get a value for both *)
if f2↑.ftype then t := feval(f2); (* if they're frames *)
c1 := f1↑.calcs; (* unfix f1 from f2 *)
c2 := nil;
b := true;
while (c1 <> nil) and b do
if c1↑.other = f2 then
begin (* found calc - splice it out of list *)
b := false;
if c2 = nil then f1↑.calcs := c1↑.next else c2↑.next := c1↑.next;
if not c1↑.tvarp then upTrans(c1↑.tval,nil); (* release old trans values *)
relNode(c1); (* done with calc node *)
end
else begin c2 := c1; c1 := c1↑.next end; (* try next *)
c1 := f2↑.calcs; (* now unfix f2 from f1 *)
c2 := nil;
b := true;
while (c1 <> nil) and b do
if c1↑.other = f1 then
begin (* found calc - splice it out of list *)
b := false;
if c2 = nil then f2↑.calcs := c1↑.next else c2↑.next := c1↑.next;
if not c1↑.tvarp then upTrans(c1↑.tval,nil); (* release old trans values *)
relNode(c1); (* done with calc node *)
end
else begin c2 := c1; c1 := c1↑.next end; (* try next *)
if not f1↑.ftype then b := unfixaux(f2,0) (* f2 no longer dynamic *)
else if not f2↑.ftype then b := unfixaux(f1,0) (* f1 no longer dynamic *)
else if f1↑.dev <> nil then (* both currently dynamic *)
if f1↑.dcntr < f2↑.dcntr then b := unfixaux(f2,f1↑.dcntr) (* unmark f2 *)
else b := unfixaux(f1,f2↑.dcntr); (* unmark f1 *)
end;
procedure doAffix;
var f1, f2: framep; ev: enventryp; c1, c2: nodep; t: transp; b: boolean;
begin
with curInt↑ do
begin (* stack has subscripts for frame1, frame2 & byvar & atexp value *)
ev := gtVarn(spc↑.frame1); (* access variable *)
f1 := ev↑.f;
ev := gtVarn(spc↑.frame2); (* access variable *)
f2 := ev↑.f;
if spc↑.byvar <> nil then
ev := gtVarn(spc↑.byvar) (* access variable *)
else ev := nil;
if spc↑.atexp <> nil then
begin
c1 := pop; (* get at expression value *)
t := c1↑.t; (* save it for later *)
relNode(c1); (* release node *)
end
else t := ttmul(feval(f1),tinvrt(feval(f2))); (* need to compute it *)
c1 := f1↑.calcs; (* see if frames are already affixed *)
b := true;
while b and (c1 <> nil) do
if c1↑.other = f2 then b := false else c1 := c1↑.next;
if c1 <> nil then (* currently affixed *)
begin
c2 := f2↑.calcs; (* find its mate *)
while c2↑.other <> f1 do c2 := c2↑.next;
if (not c1↑.tvarp) and (spc↑.byvar <> nil) then
begin (* if old affixment was direct and new one isn't *)
upTrans(c1↑.tval,nil); (* release old trans values *)
upTrans(c2↑.tval,nil);
end;
end
else
begin (* get a pair of calc nodes *)
c1 := newNode;
c2 := newNode;
c1↑.ntype := calcnode; (* indicate that we're a calc *)
c2↑.ntype := calcnode;
c1↑.other := f2; (* fill in other field *)
c2↑.other := f1;
c1↑.next := f1↑.calcs; (* link us to list of calcs *)
f1↑.calcs := c1;
c2↑.next := f2↑.calcs;
f2↑.calcs := c2;
c1↑.tval := nil; (* don't have a value yet *)
c2↑.tval := nil;
end;
c1↑.frame1 := true; (* say who's who *)
c2↑.frame1 := false;
c1↑.rigid := spc↑.rigid; (* remember what type of affixment *)
c2↑.rigid := spc↑.rigid;
b := ev <> nil; (* trans by var given? *)
c1↑.tvarp := b;
c2↑.tvarp := b;
if b then
begin (* indirect trans pointer *)
upTrans(ev↑.t,t); (* store away relation trans *)
c1↑.tvar := ev; (* and pointers to trans var *)
c2↑.tvar := ev;
end
else
begin (* direct trans *)
upTrans(c1↑.tval,t); (* store away relation trans *)
upTrans(c2↑.tval,t);
end;
b := false; (* assume no conflict *)
if not f1↑.ftype then (* f1 is a device *)
if not f2↑.ftype then b := f1 <> f2 (* f2 is also a device! *)
else
if f2↑.dev <> nil then b := f2↑.dev <> f1 (* f2 already dynamic *)
else affixaux(f2,f1,1) (* f2 now dynamic *)
else (* f1 is a frame *)
if not f2↑.ftype then (* f2 is a device *)
if f1↑.dev <> nil then b := f1↑.dev <> f2 (* f1 already dynamic *)
else affixaux(f1,f2,1) (* f1 now dynamic *)
else (* both frames *)
if f1↑.dev <> nil then (* f1 is dynamic *)
if f2↑.dev <> nil then b := f1↑.dev <> f2↑.dev (* both dynamic *)
else affixaux(f2,f1↑.dev,f1↑.dcntr+1) (* f2 now dynamic *)
else
if f2↑.dev <> nil then affixaux(f1,f2↑.dev,f2↑.dcntr+1); (* f1 now dynamic *)
if b then
begin
pp20L('Can''t have an affixm',20); pp20('ent chain connecting',20);
pp20(' two devices togethe',20); pp5('r! ',2); ppLine;
end;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doUnfix;
var f1, f2: framep; ev: enventryp;
begin
with curInt↑ do
begin (* subscripts for frame1 & frame2 on stack *)
ev := gtVarn(spc↑.frame1); (* access variable *)
f1 := ev↑.f;
ev := gtVarn(spc↑.frame2); (* access variable *)
f2 := ev↑.f;
unfix(f1,f2); (* now unfix them *)
mode := 0;
spc := spc↑.next;
end;
end;
(* aux routines for motions: forcebits, getMechbits, moveStart, moveEnd, moveRetry *)
function forcebits(fn: nodep; var negv: boolean): integer;
var vec: vectorp; fbits: integer;
begin
fbits := XFORCE;
negv := false;
vec := nil;
with fn↑.fvec↑ do
if ntype = leafnode then vec := pcval↑.v (* first check if axis vector *)
else if op = vnegop then (* or negative axis vector *)
if arg1↑.ntype = leafnode then
begin vec := arg1↑.pcval↑.v; negv := true end;
if vec = yhat then fbits := YFORCE
else if vec = zhat then fbits := ZFORCE
else if vec <> xhat then negv := false;
if fn↑.ftype >= torque then fbits := fbits + XMOMENT;
forcebits := fbits;
end;
function getMechbits: integer;
var i: integer;
begin
with curInt↑ do
if mech = nil then i := GARMDEV (* default to green arm *)
else if mech↑.ftype then
if mech↑.dev <> nil then i := mech↑.dev↑.mech
else i := GARMDEV (* default to green arm *)
else i := mech↑.mech;
getMechbits := i;
end;
procedure moveStart;
var cl: nodep; st: statementp;
begin (* enable all cmons *)
cl := curInt↑.spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for condition monitors to enable *)
st := nil;
with cl↑ do
if ntype = cmonnode then
begin if not (cmon↑.deferCm or errHandlerp) then st := cmon end
else if (ntype = viaptnode) or (ntype = byptnode) then st := vcode
else if (ntype = deprnode) or (ntype = apprnode) then st := code;
if st <> nil then
begin
with st↑.cdef↑ do
cmonEnable(getVar(level,offset)); (* enable cmon control block *)
end;
cl := cl↑.next;
end;
end;
procedure moveEnd;
var cl, val: nodep; st, err: statementp; e: enventryp; ev: eventp; fr: framep;
mechbits, errbits, angle, i: integer; errval: errortypes;
b: boolean; ch: char; kludge: interr;
begin (* disable all cmons, end of motion cleanup, error checking etc. *)
with curInt↑ do
begin
b := true;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for condition monitors to disable *)
st := nil;
with cl↑ do
if (ntype = cmonnode) and not errHandlerp then st := cmon
else if (ntype = viaptnode) or (ntype = byptnode) then st := vcode
else if (ntype = deprnode) or (ntype = apprnode) then st := code;
if st <> nil then
begin
with st↑.cdef↑ do
e := getVar(level,offset); (* get cmon control block *)
if e↑.c↑.running then b := false (* is it running now? *)
else cmonDisable(e↑.c); (* if not disabled it *)
end;
cl := cl↑.next;
end;
if not b then sleep(30) (* wait for cmon's to finish *)
else
begin (* all cmon's are now done *)
if mech↑.ftype then (* get offset of device error variable *)
if mech↑.dev <> nil then i := mech↑.dev↑.vari↑.offset + 1
else i := 1 (* assume garm *)
else i := mech↑.vari↑.offset + 1;
if version = 10 then
begin (* for simulation version *)
push(newNode);
with sp↑ do
begin ntype := leafnode; ltype := svaltype; s := 0.0 end;
end;
errbits := round(sp↑.s); (* remember error value *)
(* Since losing Pascal doesn't have an inverse for ord *)
kludge.i := errbits div 128; (* recover error type *)
errval := kludge.err;
angle := errbits mod 128; (* also bad angles (if applicable) *)
errbits := errbits - angle; (* strip out angle info *)
setVal(0,i); (* now pop it off stack & store it away *)
err := nil;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin (* check for error checker to run *)
with cl↑ do
if (ntype = cmonnode) and errHandlerp then
begin
val := getNval(cmon↑.oncond↑.eexpr,b); (* get error bits to check *)
if errbits = round(val↑.s) then err := cmon↑.conclusion;
if b then relnode(val);
end;
cl := cl↑.next;
end;
mode := 0; (* get ready for next statement *)
if errbits <> 0 then (* was there an error? *)
if err <> nil then
begin (* run error checker *)
spc := err;
end
else
begin (* print error message *)
if mech = nil then fr := garm
else if mech↑.ftype then (* first tell what device *)
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
with fr↑.vari↑.name↑ do prntStrng(length,name);
pp5(' - ',3);
ppArmError(errval,angle);
pp20L('"P" to proceed, "R" ',20); pp20('to retry the motion ',19);
b := (spc↑.stype <> operatetype) and (spc↑.stype <> centertype);
if b then
begin pp20(', "F" to move direct',20); pp20('ly to destination ',17) end;
pp20L(' or "B" to break to',20); pp20(' debugger: ',11);
ppOutNow;
mode := 4;
curInt↑.next := readQueue; (* *** should check that no other *)
readQueue := curInt; (* process is waiting, but... *** *)
curInt↑.status := inputqueue;
curInt := nil;
resched := true;
end
else
begin (* all ok - move on to next statement *)
spc := spc↑.next;
end
end
end;
end;
procedure moveRetry;
var ch: ascii; ev: eventp; mechbits,i: integer; fr: framep;
cl: nodep; b: boolean;
begin
with curInt↑ do
begin
mode := 0;
inputReady := false;
ch := inputLine[1]; (* what does luser want to do now? *)
if ord(ch) >= smallA then
ch := chr(ord(ch) - ord(' ')); (* convert to uppercase *)
if ch = 'B' then running := false (* break to debugger, proceed will retry *)
else if ch = 'P' then spc := spc↑.next (* move on to next statement *)
(* else if ch = 'R' then nothing to do *)
else if (ch = 'F') and
(spc↑.stype <> operatetype) and (spc↑.stype <> centertype) then
begin
mode := 3;
ev := getEvent; (* event to use when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
b := true;
cl := spc↑.clauses;
while (cl <> nil) and b do (* see if destination specified *)
begin b := cl↑.ntype = destnode; cl := cl↑.next end;
with msg↑ do
begin
cmd := movehdrcmd;
dev := mechbits;
bits := Nullingcb + Durlbcb; (* nonulling & duration *)
evt := ev;
dur := 5.0; (* default time of 5 seconds *)
sfac := 1.0;
if mech = nil then fr := garm
else if mech↑.ftype then
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
if spc↑.stype = movetype then
begin
n := 1; (* only one segment *)
sendCmd; (* send over move header *)
cmd := movesegcmd;
if not b then bits := Destptcb else bits := Byptcb + Destptcb;
sendTrans(fr↑.tdest); (* send over destination point *)
end
else if spc↑.stype = jtmovetype then
begin
n := 1; (* only one segment *)
i := round(fr↑.tdest↑.val[1,2]); (* get number of joint *)
case i of
1: bits := Nullingcb + Durlbcb + Joint1cb;
2: bits := Nullingcb + Durlbcb + Joint2cb;
3: bits := Nullingcb + Durlbcb + Joint3cb;
4: bits := Nullingcb + Durlbcb + Joint4cb;
5: bits := Nullingcb + Durlbcb + Joint5cb;
6: bits := Nullingcb + Durlbcb + Joint6cb;
end;
sendCmd; (* send over move header *)
cmd := movesegcmd;
if not b then bits := Destptcb else bits := Byptcb + Destptcb;
sendCmd;
t[i] := fr↑.tdest↑.val[1,1]; (* send over joint value *)
sendCmd;
end
else
begin
pos := fr↑.sdest;
if pos < 0.0 then
begin (* no dest specified *)
pos := 0.0;
if spc↑.stype = opentype then bits := 3 else bits := 1;
(* *** need to set Durlbcb too??? *** *)
end
else
bits := bits + Destptcb; (* indicate specifying opening *)
if mechbits = VISEDEV then
begin
cmd := operatecmd; (* vise uses an operate command *)
v2 := 0.0; (* no stop wait time *)
end;
sendCmd;
end;
end;
if version = 11 then
begin
if mechbits <> VISEDEV then signalArm; (* start it up *)
curInt↑.status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else freeEvent(ev); (* sim ver *)
end;
end;
end;
procedure doCmon;
var e: enventryp; n: nodep; b: boolean; val: nodep; r: real; fbits,i: integer;
sst: statementp;
begin
with curInt↑ do
case mode of
1: begin
if not spc↑.deferCm then (* check it's not a deferred cmon *)
begin (* need to enable the cmon *)
with spc↑.cdef↑ do
cmonEnable(getEntry(level,offset)); (* enable cmon control block *)
end;
mode := 0;
spc := spc↑.next;
end;
2: begin (* deal with ON condition *)
n := nil;
mode := 3; (* set up for doing conclusion next time *)
if spc↑.exprCm then
begin (* test if expression is now true *)
n := pop; (* get expression value *)
if n↑.s = 0.0 then
begin
sleep(20); (* no good - try again in 0.33 seconds *)
mode := 0;
end;
end
else if spc↑.oncond↑.ntype = durnode then
begin (* duration cmon *)
n := pop;
sleep(round(n↑.s * 60)); (* get wait time (in 60Hz ticks) *)
end
else if spc↑.oncond↑.ntype = forcenode then
begin (* force sensing *)
val := getNval(spc↑.oncond↑.fval,b); (* get force magnitude *)
r := val↑.s;
if b then relNode(val);
fbits := forcebits(spc↑.oncond,b);
with spc↑.oncond↑ do
begin
if (ftype = absforce) or (ftype = abstorque) then fbits := fbits + SIGMAG;
if b then begin r := -r; if frel < seqop then fbits := fbits + SIGGE end
else if frel >= seqop then fbits := fbits + SIGGE;
end;
with spc↑.conclusion↑ do
if stype = stoptype then
begin (* set FSTOP bit if no explicit frame is being stopped *)
if cf = nil then fbits := fbits + FSTOP
else if cf↑.ntype = leafnode then
begin (* need to check if same device as current mech *)
e := gtVarn(cf); (* get variable frame *)
if e↑.etype = frametype then
begin
if e↑.f = nil then i := GARMDEV (* default to green arm *)
else with e↑.f↑ do
if ftype then
if dev <> nil then i := dev↑.mech
else i := GARMDEV (* default to green arm *)
else i := mech;
if i = getMechBits then fbits := fbits + FSTOP;
end
end;
(* ** can't check if array ref since subscripts aren't on stack ** *)
end
else if stype = blocktype then
if bcode↑.stype = stoptype then
if bcode↑.cf = nil then fbits := fbits + FSTOP;
cm↑.fbits := fbits; (* remember bits in cmoncb *)
with msg↑ do
begin
cmd := forcesigcmd;
dev := getMechbits; (* deal with which arm here *)
bits := fbits;
evt := cm↑.evt;
mag := r;
end;
sendCmd;
cm↑.evt↑.count := -1;
cm↑.evt↑.waitlist := curInt; (* put us on event waitlist *)
curInt↑.status := forcewait;
curInt := nil; (* swap in someone else *)
resched := true;
end
else if spc↑.oncond↑.ntype = departingnode then
begin (* departing cmon *)
sleep(30); (* wait 0.5 seconds (in 60Hz ticks) *)
end
else
begin (* event cmon *)
if spc↑.oncond↑.ntype = arrivalnode then
with spc↑.oncond↑.evar↑ do e := getVar(level,offset)
else e := gtVarn(spc↑.oncond);
cm↑.evt := e↑.evt; (* save pointer to event we're waiting on *)
e↑.evt↑.count := e↑.evt↑.count - 1;
if e↑.evt↑.count <= 0 then (* hasn't been signalled yet, need to wait *)
begin
addPdb(e↑.evt↑.waitlist,curInt); (* add us to wait list *)
curInt↑.status := eventqueue;
curInt := nil; (* swap in someone else *)
resched := true;
end;
end;
if n <> nil then relNode(n);
end;
3: begin
mode := 0;
if cm↑.enabled then (* check that we're still enabled *)
begin
cm↑.running := true; (* set up current cmon status *)
cm↑.enabled := false;
spc := spc↑.conclusion;
end
else
begin
curInt↑.status := nullqueue;
curInt := nil; (* we should go away *)
resched := true; (* now swap in highest priority process *)
end;
end;
end;
end;
procedure doMove;
var appr,depr,dest,bydest,arrv,wobble,sfac,dur,ffr,stiff,gather,zwrist,n: nodep;
elbow,shoulder,flip,load,linear,cl,val,val1,val2: nodep;
t,tl,tb: transp; st: statementp; e: enventryp; fr: framep;
r: real; fbits,nsegs,mechbits,i,j,cmForce,useForce,jtnum: integer;
b,b1,b2,nulling,apprp,deprp,jointp: boolean; ev: eventp;
function getLoc(n: nodep): transp;
var tp: transp; b: boolean;
begin
n := getNval(n,b);
tp := n↑.t;
if b then relnode(n);
(* if t <> nil then tp := ttmul(t,tp); now done by ARM *)
getLoc := tp;
end;
function getDepr(n: nodep; b: boolean): transp;
var tp: transp; v: vectorp;
begin
if n↑.ltype = svaltype then tp := tmake(niltrans,svmul(n↑.s,zhat))
else if n↑.ltype = vectype then tp := tmake(niltrans,n↑.v)
else tp := n↑.t;
if b then relnode(n);
(* if t <> nil then tp := ttmul(t,tp); now done by ARM *)
getDepr := tp;
end;
procedure getCode(s: statementp);
var e: enventryp;
begin
if s = nil then e := nil
else
begin
with s↑ do
if stype = signaltype then e := gtVarn(event)
else e := gtVarn(oncond);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := msg↑.bits + Codecb;
end;
end;
procedure sendJt(r: real; n: nodep; b: boolean);
begin
sendCmd;
msg↑.t[jtnum] := r; (* send over joint value *)
if b then relNode(n);
sendCmd;
end;
procedure setConfigBits;
var cbits: integer;
begin
cbits := 0;
if elbow <> nil then
if elbow↑.notp then cbits := elbowcb + upcb else cbits := elbowcb;
if shoulder <> nil then
if shoulder↑.notp then cbits := cbits + shouldercb + rightcb
else cbits := cbits + shouldercb;
if flip <> nil then
if flip↑.notp then cbits := cbits + wristcb + flipcb
else cbits := cbits + wristcb;
with msg↑ do bits := bits + cbits;
end;
begin
with curInt↑ do
begin
st := spc; (* remember MOVE statement *)
jointp := st↑.stype = jtmovetype; (* is it a joint motion? *)
case mode of
1: begin (* set up force system, enable all cmons *)
if not jointp then
begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
mechbits := getMechbits;
if mech↑.ftype then (* check it's a device *)
if mech↑.dev = nil then
begin (* yow! frame that's not affixed to an arm *)
pp20L('Control frame not af',20); pp20('fixed to any device:',20);
pp20(' Assuming garm ',14); ppLine;
end;
end
else
begin
with st↑.cf↑.arg1↑.vari↑ do
e := getVar(level,offset);
mech := e↑.f; (* remember what we're moving *)
mechbits := e↑.f↑.mech;
val := getNval(st↑.cf↑.arg2↑.lval,b); (* now see which joint is wanted *)
i := round(val↑.s); (* get joint # *)
if (i < 0) or (6 < i) then (* bad joint # *)
begin
pp20L('Joint number out of ',20); pp20('range - using jt 1 ',18);
ppLine;
i := 1;
end;
if not b then
begin val := newNode; val↑.ntype := leafnode; val↑.ltype := svaltype end;
val↑.s := i; (* remember joint # for later *)
end;
ffr := nil;
stiff := nil;
gather := nil;
zwrist := nil;
cmForce := 0;
useForce := 0;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
ffnode: ffr := cl;
stiffnode: stiff := cl;
gathernode: gather := cl;
wristnode: zwrist := cl;
forcenode: useForce := useForce + 1;
cmonnode: if cmon↑.oncond↑.ntype = forcenode then cmForce := cmForce + 1;
others: begin (* don't care *) end;
end;
cl := next;
end;
if (ffr <> nil) or (cmForce + useForce > 0) or (gather <> nil) then
begin
msg↑.cmd := setccmd;
msg↑.dev := mechbits; (* tell which arm *)
msg↑.bits := FTABLE; (* assume this *)
if ffr <> nil then
begin
val1 := getNval(ffr↑.ff,b); (* get force frame value *)
if not ffr↑.csys then msg↑.bits := 0;
sendTrans(val1↑.t); (* send command & trans over *)
if b then relNode(val1);
end
else sendTrans(niltrans); (* send command & trans over *)
signalArm; (* wake up ARM servo background job *)
end;
if zwrist <> nil then b := not zwrist↑.notp
else b := (ffr <> nil) or (stiff <> nil) or (cmForce + useForce > 0);
if b then
begin
msg↑.cmd := zerowristcmd; (* tell arm servo to zero wrist *)
msg↑.dev := mechbits; (* tell which wrist *)
sendCmd;
end;
if stiff <> nil then
begin
val1 := getNval(stiff↑.fv,b1); (* get force vector *)
val2 := getNval(stiff↑.mv,b2); (* get moment vector *)
with msg↑ do
begin
cmd := setstiffcmd;
dev := mechbits; (* tell which arm *)
sendCmd; (* send first packet over *)
for i := 1 to 3 do
begin
t[i] := val1↑.v↑.val[i];
t[i+3] := val2↑.v↑.val[i];
end;
end;
sendCmd; (* send stiffnesses over *)
signalArm; (* wake up ARM servo background job *)
if b1 then killNode(val1);
if b2 then killNode(val2);
end
else if useForce > 0 then
begin (* add default stiffness *)
with msg↑ do
begin
cmd := setstiffcmd;
dev := mechbits; (* tell which arm *)
sendCmd; (* send first packet over *)
for i := 1 to 3 do
begin
t[i] := 40;
t[i+3] := 100;
end;
end;
sendCmd; (* send stiffnesses over *)
signalArm; (* wake up ARM servo background job *)
end;
if gather <> nil then
begin
with msg↑ do
begin
cmd := gathercmd;
dev := mechbits; (* tell with which arm *)
bits := gather↑.gbits;
end;
sendCmd; (* send gather command over *)
end;
if useForce > 0 then (* any bias forces? *)
begin
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if ntype = forcenode then (* check for bias forces *)
begin
val1 := getNval(cl↑.fval,b); (* get force magnitude *)
r := val1↑.s;
if b then relnode(val1);
fbits := forcebits(cl,b);
if b then r := -r;
with msg↑ do
begin
cmd := biasoncmd;
dev := mechbits; (* tell with which arm *)
bits := fbits;
mag := r;
end;
sendCmd; (* tell arm about bias force *)
end;
cl := cl↑.next;
end;
end;
moveStart; (* enable all condition monitors for move *)
if jointp then push(val);
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
if jointp then
begin val := pop; jtnum := round(val↑.s); relNode(val) end;
nsegs := 0;
if mech↑.ftype then
if mech↑.dev <> nil then fr := mech↑.dev (* get frame for device *)
else fr := garm
else fr := mech;
nulling := true; (* no nulling is the default *)
dest := nil;
bydest := nil;
wobble := nil;
sfac := nil;
dur := nil;
elbow := nil;
shoulder := nil;
flip := nil;
load := nil;
linear := nil;
arrv := nil;
appr := nil;
depr := nil;
if not jointp then
begin
apprp := true; (* assume default approach *)
deprp := fr↑.depr <> nil; (* default departure if last had approach *)
end
else
begin
apprp := false; (* joint moves don't use default deproaches *)
deprp := false;
end;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
destnode: begin dest := cl; nsegs := nsegs + 1 end;
wobblenode: wobble := cl;
elbownode: elbow := cl;
shouldernode: shoulder := cl;
flipnode: flip := cl;
loadnode: load := cl;
sfacnode: sfac := cl;
durnode: dur := cl;
linearnode: linear := cl;
nullingnode: nulling := notp;
apprnode: begin
appr := cl;
if loc = nil then apprp := false (* approach = nildeproach *)
else begin apprp := true; nsegs := nsegs + 1 end
end;
deprnode: begin
depr := cl;
if loc = nil then deprp := false (* departure = nildeproach *)
else begin deprp := true; nsegs := nsegs + 1 end
end;
viaptnode: nsegs := nsegs + 1;
byptnode: begin bydest := cl; nsegs := nsegs + 1 end;
cmonnode: if cmon↑.oncond↑.ntype = arrivalnode then arrv := cmon↑.oncond;
others: begin (* don't care *) end;
end;
cl := next;
end;
if (dest <> nil) then bydest := nil
else apprp := appr <> nil; (* no default approach if no dest *)
if mech↑.ftype then tb := feval(mech); (* get current cf position *)
if deprp and (depr = nil) then
nsegs := nsegs + 1; (* add in default departure seg *)
if apprp and (appr = nil) then
with dest↑.loc↑ do (* add default approach point *)
if ((ntype = leafnode) and (ltype = varitype)) or
((ntype = exprnode) and (op = arefop)) then
nsegs := nsegs + 1 (* add in default approach seg *)
else apprp := false; (* don't want default approach *)
if mech↑.ftype and (not jointp) then
begin (* get offset trans to take cf to arm *)
t := whereArm(mechbits); (* Get current device pos *)
t := ttmul(tb,tinvrt(t)); (* compute offset *)
end
else t := niltrans; (* no offset needed *)
with msg↑ do
begin
cmd := movehdrcmd;
dev := mechbits;
if jointp then
case jtnum of
1: bits := Joint1cb;
2: bits := Joint2cb;
3: bits := Joint3cb;
4: bits := Joint4cb;
5: bits := Joint5cb;
6: bits := Joint6cb;
others: bits := Joint1cb;
end
else bits := 0;
if nulling then bits := bits + Nullingcb;
if load <> nil then bits := bits + Loadcb;
if linear <> nil then (* straight line motion? *)
if linear↑.notp then bits := bits + Linearcb;
n := nsegs;
evt := ev;
end;
if sfac <> nil then
begin (* use local speed factor *)
val := getNval(sfac↑.clval,b);
msg↑.sfac := val↑.s;
if b then relnode(val);
end
else
begin (* use global speed factor *)
msg↑.sfac := speedfactor↑.s;
end;
if dur <> nil then (* duration *)
begin
val := getNval(dur↑.durval,b);
msg↑.dur := val↑.s;
if dur↑.durrel < seqop then i := Durlbcb
else if dur↑.durrel > seqop then i := Durubcb
else i := Dureqcb;
msg↑.bits := msg↑.bits + i;
if b then relnode(val);
end;
if wobble <> nil then (* wobble *)
begin
val := getNval(wobble↑.clval,b);
msg↑.wobble := val↑.s;
msg↑.bits := msg↑.bits + Wobblecb;
if b then relnode(val);
end;
(* tell arm we're starting a motion & what's being moved *)
if jointp then sendCmd else sendTrans(t);
if load <> nil then
with msg↑ do (* indicate load for arm *)
begin
cmd := setloadcmd;
if load↑.lcsys then bits := FTABLE (* in World or Hand? *)
else bits := FHAND;
val1 := getNval(load↑.loadval,b); (* mass of load *)
dur := val1↑.s;
if b then relnode(val1);
if load↑.loadvec <> nil then
begin
val1 := getNval(load↑.loadvec,b); (* where load is located *)
with val1↑.v↑ do
begin v1 := val[1]; v2 := val[2]; v3 := val[3] end;
if b then relnode(val1);
end
else begin v1 := 0; v2 := 0; v3 := 0 end;
sendCmd; (* tell ARM about the load *)
end;
msg↑.cmd := movesegcmd; (* now get values for trajectory points *)
if deprp then (* departure: loc & event *)
begin
msg↑.bits := Deptptcb;
setConfigBits; (* indicate any specified configuration *)
if depr = nil then tl := fr↑.depr (* default departure point *)
else
begin (* explicit departure point *)
msg↑.bits := Deptptcb + Byptcb; (* incremental motion *)
n := getNval(depr↑.loc,b);
getCode(depr↑.code);
if not jointp then tl := getDepr(n,b)
else sendJt(n↑.s,n,b);
end;
if not jointp then sendTrans(tl);
end;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if (ntype = viaptnode) or (ntype = byptnode) then
begin (* VIA or BY: loc, duration, velocity & event *)
if ntype = viaptnode then msg↑.bits := Viaptcb
else if cl = bydest then msg↑.bits := Destptcb + Byptcb
else msg↑.bits := Viaptcb + Byptcb;
setConfigBits; (* indicate any specified configuration *)
if jointp then begin val1 := getNval(via,b1); r := val1↑.s end
else if ntype = viaptnode then tb := getLoc(via)
else
begin
n := getNval(via,b);
if n↑.ltype = vectype then tb := tmake(niltrans,n↑.v) else tb := n↑.t;
if cl = bydest then uptrans(fr↑.tdest,tb); (* for finishing the move *)
if b then relnode(n);
end;
val2 := vclauses;
while val2 <> nil do (* check for any specified duration *)
if val2↑.ntype = durnode then
begin
val := getNval(val2↑.durval,b);
msg↑.dur := val↑.s;
if val2↑.durrel < seqop then i := Durlbcb
else if val2↑.durrel > seqop then i := Durubcb
else i := Dureqcb;
msg↑.bits := msg↑.bits + i;
if b then relNode(val);
val2 := nil;
end
else val2 := val2↑.next;
val2 := vclauses;
while val2 <> nil do (* check for any specified velocity *)
if val2↑.ntype = velocitynode then
begin
val := getNval(val2↑.clval,b);
msg↑.bits := msg↑.bits + Veloccb;
with val↑.v↑ do
begin
msg↑.v1 := val[1];
msg↑.v2 := val[2];
msg↑.v3 := val[3];
end;
if b then relNode(val);
val2 := nil;
end
else val2 := val2↑.next;
val2 := vclauses;
while val2 <> nil do (* finally deal with any configuration specs *)
begin
with msg↑ do
if val2↑.ntype = shouldernode then
begin
if shoulder = nil then
begin
bits := bits + Shouldercb;
if val2↑.notp then bits := bits + rightcb
end
else if val2↑.notp and (not shoulder↑.notp) then
bits := bits + rightcb
else if (not val2↑.notp) and shoulder↑.notp then
bits := bits - rightcb;
end
else if val2↑.ntype = elbownode then
begin
if elbow = nil then
begin
bits := bits + elbowcb;
if val2↑.notp then bits := bits + upcb
end
else if val2↑.notp and (not elbow↑.notp) then
bits := bits + upcb
else if (not val2↑.notp) and elbow↑.notp then
bits := bits - upcb;
end
else if val2↑.ntype = flipnode then
begin
if flip = nil then
begin
bits := bits + wristcb;
if val2↑.notp then bits := bits + flipcb
end
else if val2↑.notp and (not flip↑.notp) then
bits := bits + flipcb
else if (not val2↑.notp) and flip↑.notp then
bits := bits - flipcb;
end;
val2 := val2↑.next;
end;
getCode(cl↑.vcode);
if not jointp then sendTrans(tb) else sendJt(val1↑.s,val1,b1);
end;
cl := cl↑.next;
end;
if apprp then (* approach: loc & event *)
begin
msg↑.bits := Apprptcb;
setConfigBits; (* indicate any specified configuration *)
if appr <> nil then
begin (* explicit approach point *)
n := getNval(appr↑.loc,b);
getCode(appr↑.code);
end;
if not jointp then
begin
tb := getLoc(dest↑.loc); (* need to get destination location *)
tb↑.refcnt := tb↑.refcnt + 1; (* make sure we keep it for later *)
if appr <> nil then
begin
tl := getDepr(n,b); (* explicit approach point *)
tl := ttmul(tb,tl); (* shift to proper coord sys *)
end
else
begin (* default appoach point *)
tl := tvadd(tb,svmul(3,zhat));
(* if t <> nil then tl := ttmul(t,tl); now done by ARM *)
end;
tb↑.refcnt := tb↑.refcnt - 1;
upTrans(fr↑.appr,tl); (* save it for next motion *)
sendTrans(tl);
end
else
begin (* joint motion *)
val1 := getNval(dest↑.loc,b1); (* need to get destination location *)
r := val1↑.s;
sendJt(r + n↑.s,n,b); (* shift to proper coord sys *)
end
end
else
begin
if dest <> nil then
if not jointp then tb := getLoc(dest↑.loc) (* get dest for below *)
else begin val1 := getNval(dest↑.loc,b1); r := val1↑.s end;
upTrans(fr↑.appr,nil); (* remember no default depr for next motion *)
end;
(* destination: loc & event *)
if jointp then
begin
tb := newTrans;
tb↑.val[1,1] := r;
tb↑.val[1,2] := jtnum;
if dest = nil then uptrans(fr↑.tdest,tb); (* copy dest for later use *)
end;
if dest <> nil then
begin
uptrans(fr↑.tdest,tb); (* copy dest for later use *)
msg↑.bits := Destptcb;
setConfigBits; (* indicate any specified configuration *)
if arrv <> nil then
begin
with arrv↑.evar↑ do e := getVar(level,offset);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := Destptcb + Codecb;
end;
if not jointp then sendTrans(tb) else sendJt(val1↑.s,val1,b1);
end;
mode := 3;
beep; (* beep the terminal to warn that a move is about to start *)
if version = 11 then
begin
signalArm; (* finally let background job deal with traj *)
curInt↑.status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
if curInt <> nil then (* in case we're waiting for an error response *)
if spc = st↑.next then
begin (* doesn't appear to have been any errors *)
if mech↑.ftype then (* get frame for device *)
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
upTrans(fr↑.depr,fr↑.appr); (* update default departure point *)
end;
end;
end;
procedure doOperate;
var durcl,vel,torquecl,cl,v: nodep; e: enventryp; b,ccw: boolean; ev: eventp;
begin (* deal with driver *)
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
durcl := nil;
vel := nil;
torquecl := nil;
ccw := false;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
if ntype = durnode then durcl := cl
else if ntype = forcenode then
begin
if ftype = torque then torquecl := cl
else if ftype = angvelocity then vel := cl
end
else if ntype = cwnode then ccw := notp;
cl := next;
end;
with msg↑ do
begin
cmd := operatecmd;
dev := getMechbits;
bits := 0;
evt := ev;
dur := 5.0; (* default values *)
v1 := 60.0; (* angular velocity *)
v2 := 0.0; (* torque *)
if durcl <> nil then
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dur := v↑.s;
if b then relNode(v);
end;
if vel <> nil then
begin
v := getNval(vel↑.fval,b); (* get angular velocity value *)
v1 := v↑.s;
if b then relNode(v);
end;
if torquecl <> nil then
begin
v := getNval(torquecl↑.fval,b); (* get torque value *)
v2 := v↑.s;
if b then relNode(v);
end;
if ccw then
begin (* turning counterclockwise *)
v1 := - v1;
v2 := - v2;
end;
end;
sendCmd; (* pass info to ARM servo *)
mode := 3;
if version = 11 then
begin
curInt↑.status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doOpen; (* & doClose *)
var dest,bydest,sfac,durcl,swt,cl,v: nodep; e: enventryp; ev: eventp;
opening,dtime,sf,swtime: real; mechbits: integer; b,nulling: boolean;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := getMechbits;
(* run through clauses for dest, duration & speed factor/stop wait time specs *)
dest := nil;
bydest := nil;
durcl := nil;
sfac := nil;
swt := nil;
nulling := true; (* nonulling is the default *)
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
destnode: dest := cl;
byptnode: bydest := cl;
durnode: durcl := cl;
sfacnode: sfac := cl;
swtnode: swt := cl;
nullingnode: nulling := notp;
others: begin (* nothing to do *) end;
end;
cl := next;
end;
if sfac = nil then sf := speedfactor↑.s (* use global speed factor *)
else
begin
v := getNval(sfac↑.clval,b); (* get local speed factor value *)
sf := v↑.s;
if b then relNode(v);
end;
if durcl = nil then dtime := 0
else
begin
v := getNval(durcl↑.durval,b); (* get duration value *)
dtime := v↑.s;
if b then relNode(v);
end;
if swt = nil then swtime := 0
else
begin
v := getNval(swt↑.clval,b); (* get stop wait time value *)
swtime := v↑.s;
if b then relNode(v);
end;
if dest <> nil then
begin
v := getNval(dest↑.loc,b); (* get opening value *)
opening := v↑.s;
mech↑.sdest := opening; (* remember it *)
if b then relNode(v);
end
else if bydest <> nil then
begin
v := getNval(bydest↑.loc,b); (* get opening value *)
opening := v↑.s;
mech↑.sdest := mech↑.sdest + opening; (* remember it *)
if b then relNode(v);
end
else
begin
opening := 0;
mech↑.sdest := -1; (* so we know there was no dest *)
end;
with msg↑ do
begin
dev := mechbits;
evt := ev;
if nulling then bits := NULLINGCB else bits := 0;
if (dest <> nil) or (bydest <> nil) then
begin
pos := opening;
bits := bits + DESTPTCB; (* indicate we're specifying opening *)
if dest = nil then bits := bits + BYPTCB; (* tell ARM incremental motion *)
end
else
begin
pos := 0.0;
if spc↑.stype = opentype then bits := 3 else bits := 1;
end;
if durcl = nil then dur := 0.0
else
begin
dur := dtime;
bits := bits + DUREQCB;
end;
sfac := sf;
if mechbits = VISEDEV then
begin
cmd := operatecmd; (* vise uses an operate command *)
if swt = nil then
if dest = nil then v2 := 0.25 else v2 := 0.0 (* default values *)
else v2 := swtime;
if durcl = nil then dur := 8.0;
sendCmd;
end
else
begin
cmd := movehdrcmd; (* deal with hand *)
sendCmd;
(* signalArm; (* since movehdr normally followed by movesegs *)
end;
end;
mode := 3;
if version = 11 then
begin
curInt↑.status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doCenter;
var e: enventryp; ev: eventp;
begin
with curInt↑ do
case mode of
1: begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
moveStart; (* enable all condition monitors for move *)
mode := 2;
end;
2: begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
with msg↑ do
begin
cmd := centercmd;
dev := getMechbits;
bits := 0;
evt := ev;
end;
sendCmd; (* initiate the center operation *)
mode := 3;
if version = 11 then
begin
curInt↑.status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else freeEvent(ev); (* sim ver *)
end;
3: moveEnd; (* do end of motion cleanup, run error handler, etc. *)
4: moveRetry; (* deal with user response if there was an error *)
end;
end;
procedure doArmmagic;
var e: enventryp; ev: eventp; np: nodep; i,j,k: integer;
begin
with curInt↑ do
case mode of
1: begin
np := pop;
i := round(np↑.s); (* get # of arm magic command *)
relNode(np);
e := gtVarn(spc↑.dev); (* remember what we're moving *)
mech := e↑.f;
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
j := 0;
np := spc↑.iargs;
while np <> nil do begin np := np↑.next; j := j + 1 end; (* count args *)
with msg↑ do
begin
cmd := armmagiccmd;
n := i; (* command number *)
dev := getMechbits;
bits := j;
evt := ev;
sendCmd; (* initiate the armmagic operation *)
for i := 1 to j do
begin (* send over the arguments *)
np := pop; (* get next argument *)
if np↑.ltype = svaltype then
begin
cmd := realcmd;
dur := np↑.s
end
else if np↑.ltype = vectype then
begin
cmd := vectorcmd;
with np↑.v↑ do
begin
v1 := val[1]; (* copy vector *)
v2 := val[2];
v3 := val[3];
end
end
else if np↑.ltype = transtype then
begin
cmd := transcmd;
with np↑.t↑ do
begin
for k := 1 to 3 do begin t[k] := val[k,1]; t[k+3] := val[k,2] end;
sendCmd; (* send first packet of trans over *)
for k := 1 to 3 do begin t[k] := val[k,3]; t[k+3] := val[k,4] end;
end;
end
else
begin (* error -- must be string type *)
pp20L('ARM MAGIC can''t hand',20); pp10('le strings',10); ppLine;
cmd := realcmd;
dur := 0.0; (* send a zero instead *)
end;
sendCmd; (* send real/vector/2nd-half-of-trans over *)
killNode(np); (* flush used stack entry *)
end;
end;
signalArm; (* start things happening *)
mode := 2;
if version = 11 then
begin
status := devicewait;
curInt := nil;
resched := true; (* swap someone else in *)
end
else
begin (* sim ver *)
freeEvent(ev);
np := spc↑.oargs;
while np <> nil do
begin (* clear any subscripts off of the stack *)
with np↑.lval↑ do
if ntype <> leafnode then
with arg1↑.vari↑ do
e := getVar(level,offset); (* look up env entry *)
np := np↑.next;
end
end;
end;
2: begin
mode := 0; (* get ready for next statement *)
spc := spc↑.next;
end
end;
end;
procedure doFloat;
var mechbits: integer; e: enventryp; cl,load,val1: nodep; b: boolean;
begin
with curInt↑ do
begin
load := nil;
cl := spc↑.clauses;
while cl <> nil do (* look for LOAD clause *)
with cl↑ do
begin
if ntype = loadnode then load := cl;
cl := next;
end;
if spc↑.cf = nil then mechbits := GARMDEV (* assume GARM *)
else
begin
e := gtVarn(spc↑.cf); (* see what we're floating *)
with e↑.f↑ do
if ftype then
if dev <> nil then mechbits := dev↑.mech
else
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to float a f',20); pp20('rame not affixed to ',20);
pp20('any device: Assuming',20); pp5(' GARM',5); ppLine;
mechbits := GARMDEV;
end
else mechbits := mech;
end;
if load <> nil then
with msg↑ do (* indicate load for arm *)
begin
cmd := setloadcmd;
dev := mechbits;
if load↑.lcsys then bits := FTABLE (* in World or Hand? *)
else bits := FHAND;
val1 := getNval(load↑.loadval,b); (* mass of load *)
dur := val1↑.s;
if b then relnode(val1);
if load↑.loadvec <> nil then
begin
val1 := getNval(load↑.loadvec,b); (* where load is located *)
with val1↑.v↑ do
begin v1 := val[1]; v2 := val[2]; v3 := val[3] end;
if b then relnode(val1);
end
else begin v1 := 0; v2 := 0; v3 := 0 end;
sendCmd; (* tell ARM about the load *)
end;
with msg↑ do
begin
cmd := floatcmd;
if load <> nil then bits := Loadcb else bits := 0;
end;
beep; (* beep the terminal to warn that a float is about to start *)
sendCmd; (* tell arm servo to float device *)
mode := 0;
spc := spc↑.next;
end;
end;
procedure doStop;
var mechbits: integer; e: enventryp;
procedure complain;
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to stop fram',20); pp20('e not affixed to any',20);
pp20(' device: Assuming ga',20); pp5('rm ',2); ppLine;
mechbits := GARMDEV;
end;
begin
with curInt↑ do
begin
if spc↑.cf = nil then
if mech = nil then complain else mechbits := getMechbits (* use current mech *)
else
begin
e := gtVarn(spc↑.cf); (* see what we're stopping *)
with e↑.f↑ do
if ftype then
if dev <> nil then mechbits := dev↑.mech
else complain
else mechbits := mech;
end;
with msg↑ do
begin
cmd := stopcmd;
dev := mechbits;
end;
sendCmd; (* tell arm servo to stop device *)
mode := 0;
spc := spc↑.next;
end;
end;
procedure doRetry;
var b: boolean;
begin
with curInt↑ do
begin
if spc↑.rparent <> nil then
begin
b := true;
while b and (spc↑.olevel < getELev(env)) do
begin (* make sure all cmon's in outer environments have finished *)
b := cmonCheck;
if b then killEnv; (* flush all environments out to move *)
end;
if b then (* no cmons now running *)
begin
(* *** might need to clean up stack some here (fornodes) *** *)
spc := spc↑.rcode; (* go redo the previous motion *)
mode := 0;
end
else sleep(30); (* give cmons time to finish *)
end
else
begin
spc := spc↑.next; (* just go on to next statement *)
mode := 0;
end;
end;
end;
procedure doSetbase;
var mechbits: integer; e: enventryp;
procedure complain;
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to zero unkn',20); pp20('own wrist: assuming ',20);
pp5('GARM ',4); ppLine;
mechbits := GARMDEV;
end;
begin
with curInt↑ do
begin
if spc↑.cf = nil then complain
else
begin
e := gtVarn(spc↑.cf); (* see which wrist we're zeroing *)
with e↑.f↑ do
if ftype then (* a frame - is it affixed to a device? *)
if dev <> nil then mechbits := dev↑.mech
else complain
else (* a device *)
if not sdev then mechbits := mech
else complain; (* currently scalar devices are no good *)
end;
msg↑.cmd := zerowristcmd; (* tell ARM servo to zero wrist *)
msg↑.dev := mechbits;
sendCmd;
mode := 0;
spc := spc↑.next;
end;
end;
procedure doWrist;
var e,fv,tv: enventryp; b: boolean;
t: transp; v: vectorp; i: integer; val: nodep;
procedure complain;
begin (* yow! frame that's not affixed to a device *)
pp20L('Attempt to read unkn',20); pp20('own wrist: assuming ',20);
pp5('GARM ',4); ppLine;
i := GARMDEV;
end;
begin
with curInt↑ do
begin
if spc↑.arm = nil then complain
else
begin
e := gtVarn(spc↑.arm); (* see which wrist we're zeroing *)
with e↑.f↑ do
if ftype then (* a frame - is it affixed to a device? *)
if dev <> nil then i := dev↑.mech
else complain
else (* a device *)
if not sdev then i := mech
else complain; (* currently scalar devices are no good *)
end;
if spc↑.ff <> nil then
begin
val := getNval(spc↑.ff,b); (* get force frame value *)
t := val↑.t;
end
else begin t := niltrans; b := false end;
fv := gtVarn(spc↑.fvec); (* get where to store results *)
tv := gtVarn(spc↑.tvec);
if fv↑.v <> nil then (* flush any old values *)
with fv↑.v↑ do
begin
refcnt := refcnt - 1;
if refcnt <= 0 then relVector(fv↑.v);
end;
if tv↑.v <> nil then
with tv↑.v↑ do
begin
refcnt := refcnt - 1;
if refcnt <= 0 then relVector(tv↑.v);
end;
with msg↑ do
begin
cmd := wristcmd;
dev := i;
if spc↑.csys then bits := FTABLE else bits := FHAND;
end;
sendTrans(t); (* send command & trans over *)
signalArm; (* tell ARM *)
if b then relNode(val);
getReply(false); (* have ARM servo read wrist *)
v := newVector;
for i := 1 to 3 do v↑.val[i] := msg↑.t[i];
fv↑.v := v; (* store away force vector *)
v↑.refcnt := 1;
v := newVector;
for i := 1 to 3 do v↑.val[i] := msg↑.t[i+3];
tv↑.v := v; (* store away torque vector *)
v↑.refcnt := 1;
mode := 0;
spc := spc↑.next;
end;
end;
(* command loop *)
procedure interp(dLev: integer);
var p,pp: pdbp; n: nodep; b,breakNow: boolean; ch: ascii; minPriority: integer;
begin
debugLevel := dLev;
minPriority := 10 * debugLevel;
if curInt <> nil then curInt↑.status := nowrunning;
running := true; (* Means we're now running some process *)
if curInt = nil then resched := true
else if activeInts <> nil then
resched := curInt↑.priority < activeInts↑.priority;
breakNow := false;
escapeI := false;
inputp := 0;
inputReady := false;
msgp := False; (* Reset "messages-pending" flag *)
stime := 0; (* No time-ticks waiting yet *)
curTime := 0; (* Zero current time *)
if readQueue <> nil then
if readQueue↑.priority >= minPriority then (* must be at current level *)
with readQueue↑ do
begin (* remind user we're waiting for input *)
b := true;
if epc <> nil then
begin
b := false;
if epc↑.op = queryop then pp20L('Type Y or N: ',13)
else if epc↑.op = inscalarop then pp20L('Scalar please: ',15)
else b := true;
end;
if b then
begin
b := false;
if (spc↑.stype = prompttype) or (spc↑.stype = waittype) then
pp20L('Type P to proceed: ',19)
else if (movetype <= spc↑.stype) and (spc↑.stype <= centertype) then
begin
pp20L('"P" to proceed, "R" ',20); pp20('to retry the motion ',19);
if (spc↑.stype <> operatetype) and (spc↑.stype <> centertype) then
begin pp20(', "F" to move direct',20);
pp20('ly to destination ',17) end;
pp20L(' or B to break to d',20); pp10('ebugger: ',9);
end
else b := true;
end;
if not b then ppOutNow;
(* *** else ??? flush readQueue ??? *** *)
end;
while running do
begin
if msgp then (* any messages pending? *)
repeat (* yup - go read them *)
msgp := false; (* reset flag *)
b := getArm; (* read next message *)
if b then msgDispatch (* if we actually got one then deal with it *)
until not b; (* keep going til no more messages to read *)
if stime <> 0 then (* hack on 10 to simulate time *)
begin
stime := stime - 1;
if stime = 0 then (* time to wake up sleeping processes *)
begin
n := clkQueue; (* get waitlist node *)
clkQueue := n↑.next;
if clkQueue <> nil then stime := clkQueue↑.when; (* set stime for next *)
p := n↑.who;
while p <> nil do (* add waiting processes to activeInts list *)
begin
pp := p↑.next; (* remember where we are in list *)
p↑.status := runqueue;
addPdb(activeInts,p);
p := pp;
end;
relNode(n);
if curInt = nil then resched := true
else if activeInts↑.priority > curInt↑.priority then resched := true;
end;
end;
if resched then (* schedule highest priority process *)
begin
resched := false;
if curInt <> nil then
begin
curInt↑.status := runqueue;
addPdb(activeInts,curInt);
end;
curInt := activeInts; (* now swap in highest priority process *)
if activeInts <> nil then
begin
activeInts := activeInts↑.next;
curInt↑.next := nil;
curInt↑.status := nowrunning;
with curInt↑ do
breakNow := (mode = 0) and (spc↑.bpt or spc↑.bad);
end;
end;
if readQueue <> nil then (* is some process waiting for terminal input? *)
if readQueue↑.priority >= minPriority then (* must be at current level *)
while anyChar(ch) and (not inputReady) do
begin
if ch = chr(CR) then
begin (* process the line now *)
ppLine; (* echo it *)
inputReady := true;
if inputp = 0 then inputLine[1] := ' '; (* for empty lines *)
if curInt <> nil then
begin
curInt↑.status := runqueue;
curInt↑.next := activeInts;
activeInts := curInt;
resched := curInt↑.priority > readQueue↑.priority; (* for next time *)
end;
curInt := readQueue; (* swap input process in now *)
curInt↑.status := nowrunning;
readQueue := curInt↑.next; (* might be a lower level joker in queue *)
curInt↑.next := nil;
breakNow := false;
end
else if (ord(ch) = ctlH) or (ord(ch) = deletekey) then (* backspace/delete *)
begin
if inputp > 0 then
begin (* something to delete *)
inputLine[inputp] := ' ';
inputp := inputp - 1;
ppDelChar; (* erase last character *)
end
end
else if ch <> chr(LF) then (* ignore linefeeds *)
begin
inputp := inputp + 1; (* *** should check for array overflow *** *)
inputLine[inputp] := ch;
ppChar(ch); ppOutNow; (* echo it *)
end
end;
if (curInt <> nil) and (not breakNow) then (* something to do now *)
with curInt↑ do
if priority >= minPriority then (* must be at current level *)
if epc <> nil then evalExp (* continue evaluating current expression *)
else if curInt↑.mode = 0 then
begin (* evaluate any expressions needed by current statement *)
epc := spc↑.exprs;
mode := 1;
if spc↑.stype = untiltype then epc := nil (* evaluate condition later *)
else if spc↑.stype = cmtype then (* treat enabling a cmon specially *)
if cm = nil then epc := nil
else if cm↑.cmon <> spc then epc := nil
else mode := 2; (* we're doing the ON cond *)
end
else case spc↑.stype of (* interpret the current statement *)
progtype: doProg;
blocktype: doBlock;
coblocktype: doCoblock;
coendtype,
endtype: doEnd;
fortype: doFor;
iftype: doIf;
whiletype: doWhile;
untiltype: doUntil;
casetype: doCase;
calltype: doCall;
returntype: doReturn;
printtype: doPrint;
prompttype: doPrompt;
pausetype: doPause;
aborttype: doAbort;
saytype: doSay;
assigntype: doAssign;
signaltype: doSignal;
waittype: doWait;
enabletype: doEnable;
disabletype: doDisable;
cmtype: doCmon;
affixtype: doAffix;
unfixtype: doUnfix;
movetype,
jtmovetype: doMove;
operatetype: doOperate;
opentype,
closetype: doOpen; (* someday close may be different ... *)
centertype: doCenter;
floattype: doFloat;
stoptype: doStop;
retrytype: doRetry;
setbasetype: doSetbase;
wristtype: doWrist;
armmagictype: doArmmagic;
evaltype,
commenttype,
emptytype,
requiretype,
definetype,
declaretype,
dimdeftype: begin
if spc↑.stype = evaltype then
spc↑.aval := pop; (* get value for EDIT *)
mode := 0;
spc := spc↑.next; (* move on *)
end;
(* more??? *)
end;
if (curInt <> nil) and running then (* check if we've hit a breakpoint *)
with curInt↑ do
if priority >= minPriority then (* must be at current level *)
running := not((mode = 0) and (spc↑.bpt or spc↑.bad));
if escapeI then
begin
b := running;
if curInt = nil then running := false
else with curInt↑ do
if priority < minPriority then running := false
else if curInt↑.mode = 0 then (* ready to start some real stmnt? *)
if (spc↑.stype <> endtype) and (spc↑.stype <> coendtype) then
running := false;
if b and not running then pp20L('Escape-I interrupt ',18);
end;
end; (* repeat til done running *)
(* finish up - leave things in a clean state *)
end;
begin
end.